From d4968cd059d7cdad6e03714fe50c4d8571bd66cf Mon Sep 17 00:00:00 2001 From: Thomas Capricelli Date: Mon, 24 Aug 2009 15:13:12 +0200 Subject: [PATCH] cleaning, fixing most goto's --- unsupported/Eigen/src/NonLinear/lmder.h | 458 ++++++++++-------------- 1 file changed, 196 insertions(+), 262 deletions(-) diff --git a/unsupported/Eigen/src/NonLinear/lmder.h b/unsupported/Eigen/src/NonLinear/lmder.h index 6090bd48a..d651da7d5 100644 --- a/unsupported/Eigen/src/NonLinear/lmder.h +++ b/unsupported/Eigen/src/NonLinear/lmder.h @@ -47,9 +47,9 @@ int ei_lmder( /* check the input parameters for errors. */ if (n <= 0 || m < n || ftol < 0. || xtol < 0. || - gtol < 0. || maxfev <= 0 || factor <= 0.) { + gtol < 0. || maxfev <= 0 || factor <= 0.) goto L300; - } + if (mode == 2) for (j = 0; j < n; ++j) if (diag[j] <= 0.) goto L300; @@ -59,9 +59,8 @@ int ei_lmder( 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. */ @@ -71,283 +70,218 @@ int ei_lmder( /* beginning of the outer loop. */ -L30: + while(true) { - /* calculate the jacobian matrix. */ + /* calculate the jacobian matrix. */ - iflag = Functor::df(x, fjac); - ++njev; - if (iflag < 0) { - goto L300; - } + 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) { - goto L40; - } - iflag = 0; - if ((iter - 1) % nprint == 0) { - iflag = Functor::debug(x, fvec, fjac); - } - 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, fjac); + 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: */ + + /* 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; } -L120: - fjac(j,j) = wa1[j]; - qtf[j] = wa4[j]; - /* L130: */ + 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, fjac); - } return info; }