From 63071ac968fc44884df0ebda8d003b946f580c72 Mon Sep 17 00:00:00 2001 From: Thomas Capricelli Date: Mon, 24 Aug 2009 16:05:57 +0200 Subject: [PATCH] cleaning, removing goto's, uniformization (try to reduce diff between hybr[dj].h or lm[der,dif,str].h as much as possible), for future merging. --- unsupported/Eigen/src/NonLinear/hybrd.h | 66 ++-- unsupported/Eigen/src/NonLinear/hybrj.h | 64 ++-- unsupported/Eigen/src/NonLinear/lmder.h | 55 ++- unsupported/Eigen/src/NonLinear/lmdif.h | 458 ++++++++++------------ unsupported/Eigen/src/NonLinear/lmstr.h | 490 ++++++++++-------------- 5 files changed, 493 insertions(+), 640 deletions(-) diff --git a/unsupported/Eigen/src/NonLinear/hybrd.h b/unsupported/Eigen/src/NonLinear/hybrd.h index 4d0416917..d6d0f02ea 100644 --- a/unsupported/Eigen/src/NonLinear/hybrd.h +++ b/unsupported/Eigen/src/NonLinear/hybrd.h @@ -89,7 +89,7 @@ int ei_hybrd( while (true) { jeval = true; - /* calculate the jacobian matrix. */ + /* calculate the jacobian matrix. */ iflag = ei_fdjac1(x, fvec, fjac, nb_of_subdiagonals, nb_of_superdiagonals, epsfcn, wa1, wa2); @@ -97,12 +97,12 @@ int ei_hybrd( if (iflag < 0) break; - /* compute the qr factorization of the jacobian. */ + /* compute the qr factorization of the jacobian. */ ei_qrfac(n, n, fjac.data(), fjac.rows(), false, iwa, 1, wa1.data(), wa2.data()); - /* on the first iteration and if mode is 1, scale according */ - /* to the norms of the columns of the initial jacobian. */ + /* 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) @@ -112,8 +112,8 @@ int ei_hybrd( diag[j] = 1.; } - /* on the first iteration, calculate the norm of the scaled x */ - /* and initialize the step bound delta. */ + /* on the first iteration, calculate the norm of the scaled x */ + /* and initialize the step bound delta. */ wa3 = diag.cwise() * x; xnorm = wa3.stableNorm(); @@ -122,7 +122,7 @@ int ei_hybrd( delta = factor; } - /* form (q transpose)*fvec and store in qtf. */ + /* form (q transpose)*fvec and store in qtf. */ qtf = fvec; for (j = 0; j < n; ++j) @@ -135,7 +135,7 @@ int ei_hybrd( qtf[i] += fjac(i,j) * temp; } - /* copy the triangular factor of the qr factorization into r. */ + /* copy the triangular factor of the qr factorization into r. */ sing = false; for (j = 0; j < n; ++j) { @@ -150,20 +150,20 @@ int ei_hybrd( sing = true; } - /* accumulate the orthogonal factor in fjac. */ + /* accumulate the orthogonal factor in fjac. */ ei_qform(n, n, fjac.data(), fjac.rows(), wa1.data()); - /* rescale if necessary. */ + /* rescale if necessary. */ /* Computing MAX */ if (mode != 2) diag = diag.cwise().max(wa2); - /* beginning of the inner loop. */ + /* beginning of the inner loop. */ while (true) { - /* if requested, call Functor::f to enable printing of iterates. */ + /* if requested, call Functor::f to enable printing of iterates. */ if (nprint > 0) { iflag = 0; @@ -173,23 +173,23 @@ int ei_hybrd( goto L300; } - /* determine the direction p. */ + /* determine the direction p. */ ei_dogleg(R, diag, qtf, delta, wa1); - /* store the direction p and x + p. calculate the norm of p. */ + /* store the direction p and x + p. calculate the norm of p. */ wa1 = -wa1; wa2 = x + wa1; wa3 = diag.cwise() * wa1; pnorm = wa3.stableNorm(); - /* on the first iteration, adjust the initial step bound. */ + /* on the first iteration, adjust the initial step bound. */ if (iter == 1) delta = std::min(delta,pnorm); - /* evaluate the function at x + p and calculate its norm. */ + /* evaluate the function at x + p and calculate its norm. */ iflag = Functor::f(wa2, wa4); ++nfev; @@ -197,13 +197,13 @@ int ei_hybrd( goto L300; fnorm1 = wa4.stableNorm(); - /* compute the scaled actual reduction. */ + /* compute the scaled actual reduction. */ actred = -1.; if (fnorm1 < fnorm) /* Computing 2nd power */ actred = 1. - ei_abs2(fnorm1 / fnorm); - /* compute the scaled predicted reduction. */ + /* compute the scaled predicted reduction. */ l = 0; for (i = 0; i < n; ++i) { @@ -219,14 +219,14 @@ int ei_hybrd( if (temp < fnorm) /* Computing 2nd power */ prered = 1. - ei_abs2(temp / fnorm); - /* compute the ratio of the actual to the predicted */ - /* reduction. */ + /* compute the ratio of the actual to the predicted */ + /* reduction. */ ratio = 0.; if (prered > 0.) ratio = actred / prered; - /* update the step bound. */ + /* update the step bound. */ if (ratio < Scalar(.1)) { ncsuc = 0; @@ -242,10 +242,10 @@ int ei_hybrd( } } - /* test for successful iteration. */ + /* test for successful iteration. */ if (ratio >= Scalar(1e-4)) { - /* successful iteration. update x, fvec, and their norms. */ + /* successful iteration. update x, fvec, and their norms. */ x = wa2; wa2 = diag.cwise() * x; fvec = wa4; @@ -254,7 +254,7 @@ int ei_hybrd( ++iter; } - /* determine the progress of the iteration. */ + /* determine the progress of the iteration. */ ++nslow1; if (actred >= Scalar(.001)) @@ -264,14 +264,14 @@ int ei_hybrd( if (actred >= Scalar(.1)) nslow2 = 0; - /* test for convergence. */ + /* test for convergence. */ if (delta <= xtol * xnorm || fnorm == 0.) info = 1; if (info != 0) goto L300; - /* tests for termination and stringent tolerances. */ + /* tests for termination and stringent tolerances. */ if (nfev >= maxfev) info = 2; @@ -285,14 +285,14 @@ int ei_hybrd( if (info != 0) goto L300; - /* criterion for recalculating jacobian approximation */ - /* by forward differences. */ + /* criterion for recalculating jacobian approximation */ + /* by forward differences. */ if (ncfail == 2) break; - /* calculate the rank one modification to the jacobian */ - /* and update qtf if necessary. */ + /* calculate the rank one modification to the jacobian */ + /* and update qtf if necessary. */ for (j = 0; j < n; ++j) { sum = wa4.dot(fjac.col(j)); @@ -302,17 +302,17 @@ int ei_hybrd( qtf[j] = sum; } - /* compute the qr factorization of the updated jacobian. */ + /* compute the qr factorization of the updated jacobian. */ ei_r1updt(n, n, R.data(), R.size(), wa1.data(), wa2.data(), wa3.data(), &sing); ei_r1mpyq(n, n, fjac.data(), fjac.rows(), wa2.data(), wa3.data()); ei_r1mpyq(1, n, qtf.data(), 1, wa2.data(), wa3.data()); - /* end of the inner loop. */ + /* end of the inner loop. */ jeval = false; } - /* end of the outer loop. */ + /* end of the outer loop. */ } L300: /* termination, either normal or user imposed. */ diff --git a/unsupported/Eigen/src/NonLinear/hybrj.h b/unsupported/Eigen/src/NonLinear/hybrj.h index 4276d804b..ba4b8493a 100644 --- a/unsupported/Eigen/src/NonLinear/hybrj.h +++ b/unsupported/Eigen/src/NonLinear/hybrj.h @@ -78,19 +78,19 @@ int ei_hybrj( while (true) { jeval = true; - /* calculate the jacobian matrix. */ + /* calculate the jacobian matrix. */ iflag = Functor::df(x, fjac); ++njev; if (iflag < 0) break; - /* compute the qr factorization of the jacobian. */ + /* compute the qr factorization of the jacobian. */ ei_qrfac(n, n, fjac.data(), fjac.rows(), false, iwa, 1, wa1.data(), wa2.data()); - /* on the first iteration and if mode is 1, scale according */ - /* to the norms of the columns of the initial jacobian. */ + /* 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) @@ -100,8 +100,8 @@ int ei_hybrj( diag[j] = 1.; } - /* on the first iteration, calculate the norm of the scaled x */ - /* and initialize the step bound delta. */ + /* on the first iteration, calculate the norm of the scaled x */ + /* and initialize the step bound delta. */ wa3 = diag.cwise() * x; xnorm = wa3.stableNorm(); @@ -110,7 +110,7 @@ int ei_hybrj( delta = factor; } - /* form (q transpose)*fvec and store in qtf. */ + /* form (q transpose)*fvec and store in qtf. */ qtf = fvec; for (j = 0; j < n; ++j) @@ -123,7 +123,7 @@ int ei_hybrj( qtf[i] += fjac(i,j) * temp; } - /* copy the triangular factor of the qr factorization into r. */ + /* copy the triangular factor of the qr factorization into r. */ sing = false; for (j = 0; j < n; ++j) { @@ -138,20 +138,20 @@ int ei_hybrj( sing = true; } - /* accumulate the orthogonal factor in fjac. */ + /* accumulate the orthogonal factor in fjac. */ ei_qform(n, n, fjac.data(), fjac.rows(), wa1.data()); - /* rescale if necessary. */ + /* rescale if necessary. */ /* Computing MAX */ if (mode != 2) diag = diag.cwise().max(wa2); - /* beginning of the inner loop. */ + /* beginning of the inner loop. */ while (true) { - /* if requested, call Functor::f to enable printing of iterates. */ + /* if requested, call Functor::f to enable printing of iterates. */ if (nprint > 0) { iflag = 0; @@ -161,23 +161,23 @@ int ei_hybrj( goto L300; } - /* determine the direction p. */ + /* determine the direction p. */ ei_dogleg(R, diag, qtf, delta, wa1); - /* store the direction p and x + p. calculate the norm of p. */ + /* store the direction p and x + p. calculate the norm of p. */ wa1 = -wa1; wa2 = x + wa1; wa3 = diag.cwise() * wa1; pnorm = wa3.stableNorm(); - /* on the first iteration, adjust the initial step bound. */ + /* on the first iteration, adjust the initial step bound. */ if (iter == 1) delta = std::min(delta,pnorm); - /* evaluate the function at x + p and calculate its norm. */ + /* evaluate the function at x + p and calculate its norm. */ iflag = Functor::f(wa2, wa4); ++nfev; @@ -185,13 +185,13 @@ int ei_hybrj( goto L300; fnorm1 = wa4.stableNorm(); - /* compute the scaled actual reduction. */ + /* compute the scaled actual reduction. */ actred = -1.; if (fnorm1 < fnorm) /* Computing 2nd power */ actred = 1. - ei_abs2(fnorm1 / fnorm); - /* compute the scaled predicted reduction. */ + /* compute the scaled predicted reduction. */ l = 0; for (i = 0; i < n; ++i) { @@ -207,14 +207,14 @@ int ei_hybrj( if (temp < fnorm) /* Computing 2nd power */ prered = 1. - ei_abs2(temp / fnorm); - /* compute the ratio of the actual to the predicted */ - /* reduction. */ + /* compute the ratio of the actual to the predicted */ + /* reduction. */ ratio = 0.; if (prered > 0.) ratio = actred / prered; - /* update the step bound. */ + /* update the step bound. */ if (ratio < Scalar(.1)) { ncsuc = 0; @@ -230,10 +230,10 @@ int ei_hybrj( } } - /* test for successful iteration. */ + /* test for successful iteration. */ if (ratio >= Scalar(1e-4)) { - /* successful iteration. update x, fvec, and their norms. */ + /* successful iteration. update x, fvec, and their norms. */ x = wa2; wa2 = diag.cwise() * x; fvec = wa4; @@ -242,7 +242,7 @@ int ei_hybrj( ++iter; } - /* determine the progress of the iteration. */ + /* determine the progress of the iteration. */ ++nslow1; if (actred >= Scalar(.001)) @@ -252,14 +252,14 @@ int ei_hybrj( if (actred >= Scalar(.1)) nslow2 = 0; - /* test for convergence. */ + /* test for convergence. */ if (delta <= xtol * xnorm || fnorm == 0.) info = 1; if (info != 0) goto L300; - /* tests for termination and stringent tolerances. */ + /* tests for termination and stringent tolerances. */ if (nfev >= maxfev) info = 2; @@ -273,13 +273,13 @@ int ei_hybrj( if (info != 0) goto L300; - /* criterion for recalculating jacobian. */ + /* criterion for recalculating jacobian. */ if (ncfail == 2) break; - /* calculate the rank one modification to the jacobian */ - /* and update qtf if necessary. */ + /* calculate the rank one modification to the jacobian */ + /* and update qtf if necessary. */ for (j = 0; j < n; ++j) { sum = wa4.dot(fjac.col(j)); @@ -289,17 +289,17 @@ int ei_hybrj( qtf[j] = sum; } - /* compute the qr factorization of the updated jacobian. */ + /* compute the qr factorization of the updated jacobian. */ ei_r1updt(n, n, R.data(), R.size(), wa1.data(), wa2.data(), wa3.data(), &sing); ei_r1mpyq(n, n, fjac.data(), fjac.rows(), wa2.data(), wa3.data()); ei_r1mpyq(1, n, qtf.data(), 1, wa2.data(), wa3.data()); - /* end of the inner loop. */ + /* end of the inner loop. */ jeval = false; } - /* end of the outer loop. */ + /* end of the outer loop. */ } L300: /* termination, either normal or user imposed. */ diff --git a/unsupported/Eigen/src/NonLinear/lmder.h b/unsupported/Eigen/src/NonLinear/lmder.h index d651da7d5..1f0e5b20b 100644 --- a/unsupported/Eigen/src/NonLinear/lmder.h +++ b/unsupported/Eigen/src/NonLinear/lmder.h @@ -46,8 +46,7 @@ int ei_lmder( /* check the input parameters for errors. */ - if (n <= 0 || m < n || ftol < 0. || xtol < 0. || - gtol < 0. || maxfev <= 0 || factor <= 0.) + if (n <= 0 || m < n || ftol < 0. || xtol < 0. || gtol < 0. || maxfev <= 0 || factor <= 0.) goto L300; if (mode == 2) @@ -70,16 +69,16 @@ int ei_lmder( /* beginning of the outer loop. */ - while(true) { + while (true) { - /* calculate the jacobian matrix. */ + /* calculate the jacobian matrix. */ iflag = Functor::df(x, fjac); ++njev; if (iflag < 0) break; - /* if requested, call Functor::f to enable printing of iterates. */ + /* if requested, call Functor::f to enable printing of iterates. */ if (nprint > 0) { iflag = 0; @@ -89,13 +88,13 @@ int ei_lmder( break; } - /* compute the qr factorization of the jacobian. */ + /* compute the qr factorization of the jacobian. */ ei_qrfac(m, n, fjac.data(), fjac.rows(), true, ipvt.data(), n, wa1.data(), wa2.data()); ipvt.cwise()-=1; // qrfac() creates ipvt with fortran convetion (1->n), convert it to c (0->n-1) - /* on the first iteration and if mode is 1, scale according */ - /* to the norms of the columns of the initial jacobian. */ + /* 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) @@ -104,8 +103,10 @@ int ei_lmder( if (wa2[j] == 0.) diag[j] = 1.; } - /* on the first iteration, calculate the norm of the scaled x */ - /* and initialize the step bound delta. */ + + /* on the first iteration, calculate the norm of the scaled x */ + /* and initialize the step bound delta. */ + wa3 = diag.cwise() * x; xnorm = wa3.stableNorm(); delta = factor * xnorm; @@ -113,8 +114,8 @@ int ei_lmder( delta = factor; } - /* form (q transpose)*fvec and store the first n components in */ - /* qtf. */ + /* form (q transpose)*fvec and store the first n components in */ + /* qtf. */ wa4 = fvec; for (j = 0; j < n; ++j) { @@ -130,7 +131,7 @@ int ei_lmder( qtf[j] = wa4[j]; } - /* compute the norm of the scaled gradient. */ + /* compute the norm of the scaled gradient. */ gnorm = 0.; if (fnorm != 0.) @@ -145,21 +146,21 @@ int ei_lmder( } } - /* test for convergence of the gradient norm. */ + /* test for convergence of the gradient norm. */ - if (gnorm <= gtol) { + if (gnorm <= gtol) info = 4; - } if (info != 0) break; - /* rescale if necessary. */ + /* rescale if necessary. */ if (mode != 2) /* Computing MAX */ diag = diag.cwise().max(wa2); - /* beginning of the inner loop. */ + /* beginning of the inner loop. */ do { + /* determine the levenberg-marquardt parameter. */ ei_lmpar(fjac, ipvt, diag, qtf, delta, par, wa1, wa2); @@ -173,9 +174,8 @@ int ei_lmder( /* on the first iteration, adjust the initial step bound. */ - if (iter == 1) { + if (iter == 1) delta = std::min(delta,pnorm); - } /* evaluate the function at x + p and calculate its norm. */ @@ -198,9 +198,8 @@ int ei_lmder( for (j = 0; j < n; ++j) { l = ipvt[j]; temp = wa1[l]; - for (i = 0; i <= j; ++i) { + for (i = 0; i <= j; ++i) wa3[i] += fjac(i,j) * temp; - } } temp1 = ei_abs2(wa3.stableNorm() / fnorm); temp2 = ei_abs2(ei_sqrt(par) * pnorm / fnorm); @@ -227,12 +226,9 @@ int ei_lmder( /* Computing MIN */ delta = temp * std::min(delta, pnorm / Scalar(.1)); par /= temp; - } - else { - if (!(par != 0. && ratio < Scalar(.75))) { - delta = pnorm / Scalar(.5); - par = Scalar(.5) * par; - } + } else if (!(par != 0. && ratio < Scalar(.75))) { + delta = pnorm / Scalar(.5); + par = Scalar(.5) * par; } /* test for successful iteration. */ @@ -272,12 +268,11 @@ int ei_lmder( goto L300; /* end of the inner loop. repeat if iteration unsuccessful. */ } while (ratio < Scalar(1e-4)); - /* end of the outer loop. */ + /* end of the outer loop. */ } L300: /* termination, either normal or user imposed. */ - if (iflag < 0) info = iflag; if (nprint > 0) diff --git a/unsupported/Eigen/src/NonLinear/lmdif.h b/unsupported/Eigen/src/NonLinear/lmdif.h index 42aefb32d..70d6b76bc 100644 --- a/unsupported/Eigen/src/NonLinear/lmdif.h +++ b/unsupported/Eigen/src/NonLinear/lmdif.h @@ -45,10 +45,8 @@ int ei_lmdif( /* check the input parameters for errors. */ - if (n <= 0 || m < n || ftol < 0. || xtol < 0. || - gtol < 0. || maxfev <= 0 || factor <= 0.) { + if (n <= 0 || m < n || ftol < 0. || xtol < 0. || gtol < 0. || maxfev <= 0 || factor <= 0.) goto L300; - } if (mode == 2) for (j = 0; j < n; ++j) if (diag[j] <= 0.) goto L300; @@ -58,9 +56,8 @@ int ei_lmdif( iflag = Functor::f(x, fvec); nfev = 1; - if (iflag < 0) { + if (iflag < 0) goto L300; - } fnorm = fvec.stableNorm(); /* initialize levenberg-marquardt parameter and iteration counter. */ @@ -70,283 +67,214 @@ int ei_lmdif( /* beginning of the outer loop. */ -L30: + while (true) { - /* calculate the jacobian matrix. */ + /* calculate the jacobian matrix. */ - iflag = ei_fdjac2(x, fvec, fjac, epsfcn, wa4); - nfev += n; - if (iflag < 0) { - goto L300; - } + iflag = ei_fdjac2(x, fvec, fjac, epsfcn, wa4); + nfev += n; + if (iflag < 0) + break; - /* if requested, call Functor::f to enable printing of iterates. */ + /* if requested, call Functor::f to enable printing of iterates. */ - if (nprint <= 0) { - goto L40; - } - iflag = 0; - if ((iter - 1) % nprint == 0) { - iflag = Functor::debug(x, fvec); - } - if (iflag < 0) { - goto L300; - } -L40: - - /* compute the qr factorization of the jacobian. */ - - ei_qrfac(m, n, fjac.data(), fjac.rows(), true, ipvt.data(), n, wa1.data(), wa2.data()); - ipvt.cwise()-=1; // qrfac() creates ipvt with fortran convetion (1->n), convert it to c (0->n-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) { - goto L80; - } - if (mode == 2) { - goto L60; - } - for (j = 0; j < n; ++j) { - diag[j] = wa2[j]; - if (wa2[j] == 0.) { - diag[j] = 1.; + if (nprint > 0) { + iflag = 0; + if ((iter - 1) % nprint == 0) + iflag = Functor::debug(x, fvec); + if (iflag < 0) + break; } - /* L50: */ - } -L60: - /* on the first iteration, calculate the norm of the scaled x */ - /* and initialize the step bound delta. */ + /* compute the qr factorization of the jacobian. */ - wa3 = diag.cwise() * x; - xnorm = wa3.stableNorm(); - delta = factor * xnorm; - if (delta == 0.) { - delta = factor; - } -L80: + ei_qrfac(m, n, fjac.data(), fjac.rows(), true, ipvt.data(), n, wa1.data(), wa2.data()); + ipvt.cwise()-=1; // qrfac() creates ipvt with fortran convetion (1->n), convert it to c (0->n-1) - /* form (q transpose)*fvec and store the first n components in */ - /* qtf. */ + /* on the first iteration and if mode is 1, scale according */ + /* to the norms of the columns of the initial jacobian. */ - wa4 = fvec; - for (j = 0; j < n; ++j) { - if (fjac(j,j) == 0.) { - goto L120; + 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. */ + + wa3 = diag.cwise() * x; + xnorm = wa3.stableNorm(); + delta = factor * xnorm; + if (delta == 0.) + delta = factor; } - sum = 0.; - for (i = j; i < m; ++i) { - sum += fjac(i,j) * wa4[i]; - /* L100: */ + + /* form (q transpose)*fvec and store the first n components in */ + /* qtf. */ + + wa4 = fvec; + for (j = 0; j < n; ++j) { + if (fjac(j,j) != 0.) { + sum = 0.; + for (i = j; i < m; ++i) + sum += fjac(i,j) * wa4[i]; + temp = -sum / fjac(j,j); + for (i = j; i < m; ++i) + wa4[i] += fjac(i,j) * temp; + } + fjac(j,j) = wa1[j]; + qtf[j] = wa4[j]; } - temp = -sum / fjac(j,j); - for (i = j; i < m; ++i) { - wa4[i] += fjac(i,j) * temp; - /* L110: */ - } -L120: - fjac(j,j) = wa1[j]; - qtf[j] = wa4[j]; - /* L130: */ + + /* compute the norm of the scaled gradient. */ + + gnorm = 0.; + if (fnorm != 0.) + for (j = 0; j < n; ++j) { + l = ipvt[j]; + if (wa2[l] != 0.) { + sum = 0.; + for (i = 0; i <= j; ++i) + sum += fjac(i,j) * (qtf[i] / fnorm); + /* Computing MAX */ + gnorm = std::max(gnorm, ei_abs(sum / wa2[l])); + } + } + + /* test for convergence of the gradient norm. */ + + if (gnorm <= gtol) + info = 4; + if (info != 0) + break; + + /* rescale if necessary. */ + + if (mode != 2) /* Computing MAX */ + diag = diag.cwise().max(wa2); + + /* beginning of the inner loop. */ + do { + + /* determine the levenberg-marquardt parameter. */ + + ei_lmpar(fjac, ipvt, diag, qtf, delta, par, wa1, wa2); + + /* store the direction p and x + p. calculate the norm of p. */ + + wa1 = -wa1; + wa2 = x + wa1; + wa3 = diag.cwise() * wa1; + pnorm = wa3.stableNorm(); + + /* on the first iteration, adjust the initial step bound. */ + + if (iter == 1) + delta = std::min(delta,pnorm); + + /* evaluate the function at x + p and calculate its norm. */ + + iflag = Functor::f(wa2, wa4); + ++nfev; + if (iflag < 0) + goto L300; + fnorm1 = wa4.stableNorm(); + + /* compute the scaled actual reduction. */ + + actred = -1.; + if (Scalar(.1) * fnorm1 < fnorm) /* Computing 2nd power */ + actred = 1. - ei_abs2(fnorm1 / fnorm); + + /* compute the scaled predicted reduction and */ + /* the scaled directional derivative. */ + + wa3.fill(0.); + for (j = 0; j < n; ++j) { + l = ipvt[j]; + temp = wa1[l]; + for (i = 0; i <= j; ++i) + wa3[i] += fjac(i,j) * temp; + } + temp1 = ei_abs2(wa3.stableNorm() / fnorm); + temp2 = ei_abs2(ei_sqrt(par) * pnorm / fnorm); + /* Computing 2nd power */ + prered = temp1 + temp2 / Scalar(.5); + dirder = -(temp1 + 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 <= Scalar(.25)) { + if (actred >= 0.) + temp = Scalar(.5); + if (actred < 0.) + temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred); + if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) + temp = Scalar(.1); + /* Computing MIN */ + delta = temp * std::min(delta, pnorm / Scalar(.1)); + par /= temp; + } else if (!(par != 0. && ratio < Scalar(.75))) { + delta = pnorm / Scalar(.5); + par = Scalar(.5) * par; + } + + /* test for successful iteration. */ + + if (ratio >= Scalar(1e-4)) { + /* successful iteration. update x, fvec, and their norms. */ + x = wa2; + wa2 = diag.cwise() * x; + fvec = wa4; + xnorm = wa2.stableNorm(); + fnorm = fnorm1; + ++iter; + } + + /* tests for convergence. */ + + if (ei_abs(actred) <= ftol && prered <= ftol && Scalar(.5) * ratio <= 1.) + info = 1; + if (delta <= xtol * xnorm) + info = 2; + if (ei_abs(actred) <= ftol && prered <= ftol && Scalar(.5) * ratio <= 1. && info == 2) + info = 3; + if (info != 0) + goto L300; + + /* tests for termination and stringent tolerances. */ + + if (nfev >= maxfev) + info = 5; + if (ei_abs(actred) <= epsilon() && prered <= epsilon() && Scalar(.5) * ratio <= 1.) + info = 6; + if (delta <= epsilon() * xnorm) + info = 7; + if (gnorm <= epsilon()) + info = 8; + if (info != 0) + goto L300; + /* end of the inner loop. repeat if iteration unsuccessful. */ + } while (ratio < Scalar(1e-4)); + /* end of the outer loop. */ } - - /* compute the norm of the scaled gradient. */ - - gnorm = 0.; - if (fnorm == 0.) { - goto L170; - } - for (j = 0; j < n; ++j) { - l = ipvt[j]; - if (wa2[l] != 0.) { - sum = 0.; - for (i = 0; i <= j; ++i) - sum += fjac(i,j) * (qtf[i] / fnorm); - /* Computing MAX */ - gnorm = std::max(gnorm, ei_abs(sum / wa2[l])); - } - } -L170: - - /* test for convergence of the gradient norm. */ - - if (gnorm <= gtol) { - info = 4; - } - if (info != 0) { - goto L300; - } - - /* rescale if necessary. */ - - if (mode == 2) { - goto L190; - } - /* Computing MAX */ - diag = diag.cwise().max(wa2); -L190: - - /* beginning of the inner loop. */ - -L200: - - /* determine the levenberg-marquardt parameter. */ - - ei_lmpar(fjac, ipvt, diag, qtf, delta, par, wa1, wa2); - - /* store the direction p and x + p. calculate the norm of p. */ - - wa1 = -wa1; - wa2 = x + wa1; - wa3 = diag.cwise() * wa1; - pnorm = wa3.stableNorm(); - - /* on the first iteration, adjust the initial step bound. */ - - if (iter == 1) { - delta = std::min(delta,pnorm); - } - - /* evaluate the function at x + p and calculate its norm. */ - - iflag = Functor::f(wa2, wa4); - ++nfev; - if (iflag < 0) { - goto L300; - } - fnorm1 = wa4.stableNorm(); - - /* compute the scaled actual reduction. */ - - actred = -1.; - if (Scalar(.1) * fnorm1 < fnorm) /* Computing 2nd power */ - actred = 1. - ei_abs2(fnorm1 / fnorm); - - /* compute the scaled predicted reduction and */ - /* the scaled directional derivative. */ - - wa3.fill(0.); - for (j = 0; j < n; ++j) { - l = ipvt[j]; - temp = wa1[l]; - for (i = 0; i <= j; ++i) { - wa3[i] += fjac(i,j) * temp; - /* L220: */ - } - /* L230: */ - } - temp1 = ei_abs2(wa3.stableNorm() / fnorm); - temp2 = ei_abs2(ei_sqrt(par) * pnorm / fnorm); - /* Computing 2nd power */ - prered = temp1 + temp2 / Scalar(.5); - dirder = -(temp1 + 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 > Scalar(.25)) { - goto L240; - } - if (actred >= 0.) { - temp = Scalar(.5); - } - if (actred < 0.) { - temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred); - } - if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) - temp = Scalar(.1); - /* Computing MIN */ - delta = temp * std::min(delta, pnorm / Scalar(.1)); - par /= temp; - goto L260; -L240: - if (par != 0. && ratio < Scalar(.75)) { - goto L250; - } - delta = pnorm / Scalar(.5); - par = Scalar(.5) * par; -L250: -L260: - - /* test for successful iteration. */ - - if (ratio < Scalar(1e-4)) { - goto L290; - } - - /* successful iteration. update x, fvec, and their norms. */ - - x = wa2; - wa2 = diag.cwise() * x; - fvec = wa4; - xnorm = wa2.stableNorm(); - fnorm = fnorm1; - ++iter; -L290: - - /* tests for convergence. */ - - if (ei_abs(actred) <= ftol && prered <= ftol && Scalar(.5) * ratio <= 1.) { - info = 1; - } - if (delta <= xtol * xnorm) { - info = 2; - } - if (ei_abs(actred) <= ftol && prered <= ftol && Scalar(.5) * ratio <= 1. && info - == 2) { - info = 3; - } - if (info != 0) { - goto L300; - } - - /* tests for termination and stringent tolerances. */ - - if (nfev >= maxfev) { - info = 5; - } - if (ei_abs(actred) <= epsilon() && prered <= epsilon() && Scalar(.5) * ratio <= 1.) { - info = 6; - } - if (delta <= epsilon() * xnorm) { - info = 7; - } - if (gnorm <= epsilon()) { - info = 8; - } - if (info != 0) { - goto L300; - } - - /* end of the inner loop. repeat if iteration unsuccessful. */ - - if (ratio < Scalar(1e-4)) { - goto L200; - } - - /* end of the outer loop. */ - - goto L30; L300: /* termination, either normal or user imposed. */ - - if (iflag < 0) { + if (iflag < 0) info = iflag; - } - iflag = 0; - if (nprint > 0) { + if (nprint > 0) iflag = Functor::debug(x, fvec); - } return info; } diff --git a/unsupported/Eigen/src/NonLinear/lmstr.h b/unsupported/Eigen/src/NonLinear/lmstr.h index dab5f8c4d..ed516e5aa 100644 --- a/unsupported/Eigen/src/NonLinear/lmstr.h +++ b/unsupported/Eigen/src/NonLinear/lmstr.h @@ -46,22 +46,19 @@ int ei_lmstr( /* check the input parameters for errors. */ - if (n <= 0 || m < n || ftol < 0. || xtol < 0. || - gtol < 0. || maxfev <= 0 || factor <= 0.) { + if (n <= 0 || m < n || ftol < 0. || xtol < 0. || gtol < 0. || maxfev <= 0 || factor <= 0.) goto L340; - } if (mode == 2) for (j = 0; j < n; ++j) - if (diag[j] <= 0.) goto L300; + if (diag[j] <= 0.) goto L340; /* evaluate the function at the starting point */ /* and calculate its norm. */ iflag = Functor::f(x, fvec); nfev = 1; - if (iflag < 0) { + if (iflag < 0) goto L340; - } fnorm = fvec.stableNorm(); /* initialize levenberg-marquardt parameter and iteration counter. */ @@ -71,297 +68,230 @@ int ei_lmstr( /* beginning of the outer loop. */ -L30: + while (true) { - /* if requested, call Functor::f to enable printing of iterates. */ + /* if requested, call Functor::f to enable printing of iterates. */ - if (nprint <= 0) { - goto L40; - } - iflag = 0; - if ((iter - 1) % nprint == 0) { - iflag = Functor::debug(x, fvec, wa3); - } - if (iflag < 0) { - goto L340; - } -L40: - - /* compute the qr factorization of the jacobian matrix */ - /* calculated one row at a time, while simultaneously */ - /* forming (q transpose)*fvec and storing the first */ - /* n components in qtf. */ - - qtf.fill(0.); - fjac.fill(0.); - iflag = 2; - for (i = 0; i < m; ++i) { - if (Functor::df(x, wa3, iflag) < 0) - goto L340; - temp = fvec[i]; - ei_rwupdt(n, fjac.data(), fjac.rows(), wa3.data(), qtf.data(), &temp, wa1.data(), wa2.data()); - ++iflag; - } - ++njev; - - /* if the jacobian is rank deficient, call qrfac to */ - /* reorder its columns and update the components of qtf. */ - - sing = false; - for (j = 0; j < n; ++j) { - if (fjac(j,j) == 0.) { - sing = true; + if (nprint > 0) { + iflag = 0; + if ((iter - 1) % nprint == 0) + iflag = Functor::debug(x, fvec, wa3); + if (iflag < 0) + break; } - ipvt[j] = j; - wa2[j] = fjac.col(j).start(j).stableNorm(); - } - if (! sing) - goto L130; - ipvt.cwise()+=1; - ei_qrfac(n, n, fjac.data(), fjac.rows(), true, ipvt.data(), n, wa1.data(), wa2.data()); - ipvt.cwise()-=1; // qrfac() creates ipvt with fortran convetion (1->n), convert it to c (0->n-1) - for (j = 0; j < n; ++j) { - if (fjac(j,j) == 0.) - goto L110; - sum = 0.; - for (i = j; i < n; ++i) { - sum += fjac(i,j) * qtf[i]; + + /* compute the qr factorization of the jacobian matrix */ + /* calculated one row at a time, while simultaneously */ + /* forming (q transpose)*fvec and storing the first */ + /* n components in qtf. */ + + qtf.fill(0.); + fjac.fill(0.); + iflag = 2; + for (i = 0; i < m; ++i) { + if (Functor::df(x, wa3, iflag) < 0) + break; + temp = fvec[i]; + ei_rwupdt(n, fjac.data(), fjac.rows(), wa3.data(), qtf.data(), &temp, wa1.data(), wa2.data()); + ++iflag; } - temp = -sum / fjac(j,j); - for (i = j; i < n; ++i) { - qtf[i] += fjac(i,j) * temp; + ++njev; + + /* if the jacobian is rank deficient, call qrfac to */ + /* reorder its columns and update the components of qtf. */ + + sing = false; + for (j = 0; j < n; ++j) { + if (fjac(j,j) == 0.) { + sing = true; + } + ipvt[j] = j; + wa2[j] = fjac.col(j).start(j).stableNorm(); } -L110: - fjac(j,j) = wa1[j]; - /* L120: */ - } -L130: - - /* on the first iteration and if mode is 1, scale according */ - /* to the norms of the columns of the initial jacobian. */ - - if (iter != 1) { - goto L170; - } - if (mode == 2) { - goto L150; - } - for (j = 0; j < n; ++j) { - diag[j] = wa2[j]; - if (wa2[j] == 0.) { - diag[j] = 1.; + if (sing) { + ipvt.cwise()+=1; + ei_qrfac(n, n, fjac.data(), fjac.rows(), true, ipvt.data(), n, wa1.data(), wa2.data()); + ipvt.cwise()-=1; // qrfac() creates ipvt with fortran convetion (1->n), convert it to c (0->n-1) + for (j = 0; j < n; ++j) { + if (fjac(j,j) != 0.) { + sum = 0.; + for (i = j; i < n; ++i) + sum += fjac(i,j) * qtf[i]; + temp = -sum / fjac(j,j); + for (i = j; i < n; ++i) + qtf[i] += fjac(i,j) * temp; + } + fjac(j,j) = wa1[j]; + } } - /* L140: */ - } -L150: - /* on the first iteration, calculate the norm of the scaled x */ - /* and initialize the step bound delta. */ + /* on the first iteration and if mode is 1, scale according */ + /* to the norms of the columns of the initial jacobian. */ - wa3 = diag.cwise() * x; - xnorm = wa3.stableNorm(); - delta = factor * xnorm; - if (delta == 0.) { - delta = factor; - } -L170: + if (iter == 1) { + if (mode != 2) + for (j = 0; j < n; ++j) { + diag[j] = wa2[j]; + if (wa2[j] == 0.) + diag[j] = 1.; + } - /* compute the norm of the scaled gradient. */ + /* on the first iteration, calculate the norm of the scaled x */ + /* and initialize the step bound delta. */ - gnorm = 0.; - if (fnorm == 0.) { - goto L210; - } - for (j = 0; j < n; ++j) { - l = ipvt[j]; - if (wa2[l] != 0.) { - sum = 0.; - for (i = 0; i <= j; ++i) - sum += fjac(i,j) * (qtf[i] / fnorm); - /* Computing MAX */ - gnorm = std::max(gnorm, ei_abs(sum / wa2[l])); + wa3 = diag.cwise() * x; + xnorm = wa3.stableNorm(); + delta = factor * xnorm; + if (delta == 0.) + delta = factor; } + + /* compute the norm of the scaled gradient. */ + + gnorm = 0.; + if (fnorm != 0.) + for (j = 0; j < n; ++j) { + l = ipvt[j]; + if (wa2[l] != 0.) { + sum = 0.; + for (i = 0; i <= j; ++i) + sum += fjac(i,j) * (qtf[i] / fnorm); + /* Computing MAX */ + gnorm = std::max(gnorm, ei_abs(sum / wa2[l])); + } + } + + /* test for convergence of the gradient norm. */ + + if (gnorm <= gtol) + info = 4; + if (info != 0) + break; + + /* rescale if necessary. */ + + if (mode != 2) /* Computing MAX */ + diag = diag.cwise().max(wa2); + + /* beginning of the inner loop. */ + do { + + /* determine the levenberg-marquardt parameter. */ + + ei_lmpar(fjac, ipvt, diag, qtf, delta, par, wa1, wa2); + + /* store the direction p and x + p. calculate the norm of p. */ + + wa1 = -wa1; + wa2 = x + wa1; + wa3 = diag.cwise() * wa1; + pnorm = wa3.stableNorm(); + + /* on the first iteration, adjust the initial step bound. */ + + if (iter == 1) + delta = std::min(delta,pnorm); + + /* evaluate the function at x + p and calculate its norm. */ + + iflag = Functor::f(wa2, wa4); + ++nfev; + if (iflag < 0) + goto L340; + fnorm1 = wa4.stableNorm(); + + /* compute the scaled actual reduction. */ + + actred = -1.; + if (Scalar(.1) * fnorm1 < fnorm) /* Computing 2nd power */ + actred = 1. - ei_abs2(fnorm1 / fnorm); + + /* compute the scaled predicted reduction and */ + /* the scaled directional derivative. */ + + wa3.fill(0.); + for (j = 0; j < n; ++j) { + l = ipvt[j]; + temp = wa1[l]; + for (i = 0; i <= j; ++i) + wa3[i] += fjac(i,j) * temp; + } + temp1 = ei_abs2(wa3.stableNorm() / fnorm); + temp2 = ei_abs2(ei_sqrt(par) * pnorm / fnorm); + /* Computing 2nd power */ + prered = temp1 + temp2 / Scalar(.5); + dirder = -(temp1 + 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 <= Scalar(.25)) { + if (actred >= 0.) + temp = Scalar(.5); + if (actred < 0.) + temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred); + if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) + temp = Scalar(.1); + /* Computing MIN */ + delta = temp * std::min(delta, pnorm / Scalar(.1)); + par /= temp; + } else if (!(par != 0. && ratio < Scalar(.75))) { + delta = pnorm / Scalar(.5); + par = Scalar(.5) * par; + } + + /* test for successful iteration. */ + + if (ratio >= Scalar(1e-4)) { + /* successful iteration. update x, fvec, and their norms. */ + x = wa2; + wa2 = diag.cwise() * x; + fvec = wa4; + xnorm = wa2.stableNorm(); + fnorm = fnorm1; + ++iter; + } + + /* tests for convergence. */ + + if (ei_abs(actred) <= ftol && prered <= ftol && Scalar(.5) * ratio <= 1.) + info = 1; + if (delta <= xtol * xnorm) + info = 2; + if (ei_abs(actred) <= ftol && prered <= ftol && Scalar(.5) * ratio <= 1. && info == 2) + info = 3; + if (info != 0) + goto L340; + + /* tests for termination and stringent tolerances. */ + + if (nfev >= maxfev) + info = 5; + if (ei_abs(actred) <= epsilon() && prered <= epsilon() && Scalar(.5) * ratio <= 1.) + info = 6; + if (delta <= epsilon() * xnorm) + info = 7; + if (gnorm <= epsilon()) + info = 8; + if (info != 0) + goto L340; + /* end of the inner loop. repeat if iteration unsuccessful. */ + } while (ratio < Scalar(1e-4)); + /* end of the outer loop. */ } -L210: - - /* test for convergence of the gradient norm. */ - - if (gnorm <= gtol) { - info = 4; - } - if (info != 0) { - goto L340; - } - - /* rescale if necessary. */ - - if (mode == 2) { - goto L230; - } - /* Computing MAX */ - diag = diag.cwise().max(wa2); -L230: - - /* beginning of the inner loop. */ - -L240: - - /* determine the levenberg-marquardt parameter. */ - - ei_lmpar(fjac, ipvt, diag, qtf, delta, par, wa1, wa2); - - /* store the direction p and x + p. calculate the norm of p. */ - - wa1 = -wa1; - wa2 = x + wa1; - wa3 = diag.cwise() * wa1; - pnorm = wa3.stableNorm(); - - /* on the first iteration, adjust the initial step bound. */ - - if (iter == 1) { - delta = std::min(delta,pnorm); - } - - /* evaluate the function at x + p and calculate its norm. */ - - iflag = Functor::f(wa2, wa4); - ++nfev; - if (iflag < 0) { - goto L340; - } - fnorm1 = wa4.stableNorm(); - - /* compute the scaled actual reduction. */ - - actred = -1.; - if (Scalar(.1) * fnorm1 < fnorm) /* Computing 2nd power */ - actred = 1. - ei_abs2(fnorm1 / fnorm); - - /* compute the scaled predicted reduction and */ - /* the scaled directional derivative. */ - - wa3.fill(0.); - for (j = 0; j < n; ++j) { - l = ipvt[j]; - temp = wa1[l]; - for (i = 0; i <= j; ++i) { - wa3[i] += fjac(i,j) * temp; - /* L260: */ - } - /* L270: */ - } - temp1 = ei_abs2(wa3.stableNorm() / fnorm); - temp2 = ei_abs2(ei_sqrt(par) * pnorm / fnorm); - /* Computing 2nd power */ - prered = temp1 + temp2 / Scalar(.5); - dirder = -(temp1 + 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 > Scalar(.25)) { - goto L280; - } - if (actred >= 0.) { - temp = Scalar(.5); - } - if (actred < 0.) { - temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred); - } - if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) - temp = Scalar(.1); - /* Computing MIN */ - delta = temp * std::min(delta, pnorm / Scalar(.1)); - - par /= temp; - goto L300; -L280: - if (par != 0. && ratio < Scalar(.75)) { - goto L290; - } - delta = pnorm / Scalar(.5); - par = Scalar(.5) * par; -L290: -L300: - - /* test for successful iteration. */ - - if (ratio < Scalar(1e-4)) { - goto L330; - } - - /* successful iteration. update x, fvec, and their norms. */ - - x = wa2; - wa2 = diag.cwise() * x; - fvec = wa4; - xnorm = wa2.stableNorm(); - fnorm = fnorm1; - ++iter; -L330: - - /* tests for convergence. */ - - if (ei_abs(actred) <= ftol && prered <= ftol && Scalar(.5) * ratio <= 1.) { - info = 1; - } - if (delta <= xtol * xnorm) { - info = 2; - } - if (ei_abs(actred) <= ftol && prered <= ftol && Scalar(.5) * ratio <= 1. && info - == 2) { - info = 3; - } - if (info != 0) { - goto L340; - } - - /* tests for termination and stringent tolerances. */ - - if (nfev >= maxfev) { - info = 5; - } - if (ei_abs(actred) <= epsilon() && prered <= epsilon() && Scalar(.5) * ratio <= 1.) { - info = 6; - } - if (delta <= epsilon() * xnorm) { - info = 7; - } - if (gnorm <= epsilon()) { - info = 8; - } - if (info != 0) { - goto L340; - } - - /* end of the inner loop. repeat if iteration unsuccessful. */ - - if (ratio < Scalar(1e-4)) { - goto L240; - } - - /* end of the outer loop. */ - - goto L30; L340: /* termination, either normal or user imposed. */ - - if (iflag < 0) { + if (iflag < 0) info = iflag; - } - iflag = 0; - if (nprint > 0) { + if (nprint > 0) iflag = Functor::debug(x, fvec, wa3); - } return info; }