diff --git a/unsupported/Eigen/src/NonLinear/dogleg.h b/unsupported/Eigen/src/NonLinear/dogleg.h index cc1283847..3485c5796 100644 --- a/unsupported/Eigen/src/NonLinear/dogleg.h +++ b/unsupported/Eigen/src/NonLinear/dogleg.h @@ -20,7 +20,7 @@ void ei_dogleg( assert(n==qtb.size()); assert(n==x.size()); - /* first, calculate the gauss-newton direction. */ + /* first, calculate the gauss-newton direction. */ jj = n * (n + 1) / 2; for (k = 0; k < n; ++k) { @@ -39,17 +39,15 @@ void ei_dogleg( /* Computing MAX */ temp = std::max(temp,ei_abs(r[l])); l = l + n - i; - /* L30: */ } temp = epsmch * temp; - if (temp == 0.) { + if (temp == 0.) temp = epsmch; - } } x[j] = (qtb[j] - sum) / temp; } - /* test whether the gauss-newton direction is acceptable. */ + /* test whether the gauss-newton direction is acceptable. */ wa1.fill(0.); wa2 = diag.cwise() * x; @@ -57,8 +55,8 @@ void ei_dogleg( if (qnorm <= delta) return; - /* the gauss-newton direction is not acceptable. */ - /* next, calculate the scaled gradient direction. */ + /* the gauss-newton direction is not acceptable. */ + /* next, calculate the scaled gradient direction. */ l = 0; for (j = 0; j < n; ++j) { @@ -70,17 +68,17 @@ void ei_dogleg( wa1[j] /= diag[j]; } - /* calculate the norm of the scaled gradient and test for */ - /* the special case in which the scaled gradient is zero. */ + /* calculate the norm of the scaled gradient and test for */ + /* the special case in which the scaled gradient is zero. */ gnorm = wa1.stableNorm(); sgnorm = 0.; alpha = delta / qnorm; if (gnorm == 0.) - goto L120; + goto algo_end; - /* calculate the point along the scaled gradient */ - /* at which the quadratic is minimized. */ + /* calculate the point along the scaled gradient */ + /* at which the quadratic is minimized. */ wa1.cwise() /= diag*gnorm; l = 0; @@ -97,16 +95,15 @@ void ei_dogleg( temp = wa2.stableNorm(); sgnorm = gnorm / temp / temp; - /* test whether the scaled gradient direction is acceptable. */ + /* test whether the scaled gradient direction is acceptable. */ alpha = 0.; - if (sgnorm >= delta) { - goto L120; - } + if (sgnorm >= delta) + goto algo_end; - /* the scaled gradient direction is not acceptable. */ - /* finally, calculate the point along the dogleg */ - /* at which the quadratic is minimized. */ + /* the scaled gradient direction is not acceptable. */ + /* finally, calculate the point along the dogleg */ + /* at which the quadratic is minimized. */ bnorm = qtb.stableNorm(); temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta); @@ -114,10 +111,10 @@ void ei_dogleg( temp = temp - delta / qnorm * ei_abs2(sgnorm / delta) + ei_sqrt(ei_abs2(temp - delta / qnorm) + (1.-ei_abs2(delta / qnorm)) * (1.-ei_abs2(sgnorm / delta))); /* Computing 2nd power */ alpha = delta / qnorm * (1. - ei_abs2(sgnorm / delta)) / temp; -L120: +algo_end: - /* form appropriate convex combination of the gauss-newton */ - /* direction and the scaled gradient direction. */ + /* form appropriate convex combination of the gauss-newton */ + /* direction and the scaled gradient direction. */ temp = (1.-alpha) * std::min(sgnorm,delta); x = temp * wa1 + alpha * x; diff --git a/unsupported/Eigen/src/NonLinear/lmpar.h b/unsupported/Eigen/src/NonLinear/lmpar.h index 664c78bff..c62881c81 100644 --- a/unsupported/Eigen/src/NonLinear/lmpar.h +++ b/unsupported/Eigen/src/NonLinear/lmpar.h @@ -30,8 +30,8 @@ void ei_lmpar( Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n); - /* compute and store in x the gauss-newton direction. if the */ - /* jacobian is rank-deficient, obtain a least squares solution. */ + /* compute and store in x the gauss-newton direction. if the */ + /* jacobian is rank-deficient, obtain a least squares solution. */ nsing = n-1; for (j = 0; j < n; ++j) { @@ -54,39 +54,40 @@ void ei_lmpar( x[l] = wa1[j]; } - /* initialize the iteration counter. */ - /* evaluate the function at the origin, and test */ - /* for acceptance of the gauss-newton direction. */ + /* initialize the iteration counter. */ + /* evaluate the function at the origin, and test */ + /* for acceptance of the gauss-newton direction. */ iter = 0; wa2 = diag.cwise() * x; dxnorm = wa2.blueNorm(); fp = dxnorm - delta; - if (fp <= Scalar(0.1) * delta) - goto L220; + if (fp <= Scalar(0.1) * delta) { + par = 0; + return; + } - /* 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. */ + /* 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-1) - goto L120; - for (j = 0; j < n; ++j) { - l = ipvt[j]; - wa1[j] = diag[l] * (wa2[l] / dxnorm); + if (nsing >= n-1) { + for (j = 0; j < n; ++j) { + l = ipvt[j]; + wa1[j] = diag[l] * (wa2[l] / dxnorm); + } + for (j = 0; j < n; ++j) { + sum = 0.; + for (i = 0; i < j; ++i) + sum += r(i,j) * wa1[i]; + wa1[j] = (wa1[j] - sum) / r(j,j); + } + temp = wa1.blueNorm(); + parl = fp / delta / temp / temp; } - for (j = 0; j < n; ++j) { - sum = 0.; - for (i = 0; i < j; ++i) - sum += r(i,j) * wa1[i]; - wa1[j] = (wa1[j] - sum) / r(j,j); - } - temp = wa1.blueNorm(); - parl = fp / delta / temp / temp; -L120: - /* calculate an upper bound, paru, for the zero of the function. */ + /* calculate an upper bound, paru, for the zero of the function. */ for (j = 0; j < n; ++j) { sum = 0.; @@ -97,89 +98,82 @@ L120: } gnorm = wa1.stableNorm(); paru = gnorm / delta; - if (paru == 0.) { + if (paru == 0.) paru = dwarf / std::min(delta,Scalar(0.1)); - } - /* if the input par lies outside of the interval (parl,paru), */ - /* set par to the closer endpoint. */ + /* if the input par lies outside of the interval (parl,paru), */ + /* set par to the closer endpoint. */ par = std::max(par,parl); par = std::min(par,paru); if (par == 0.) par = gnorm / dxnorm; - /* beginning of an iteration. */ + /* beginning of an iteration. */ -L150: - ++iter; + while (true) { + ++iter; - /* evaluate the function at the current value of par. */ + /* evaluate the function at the current value of par. */ - if (par == 0.) - par = std::max(dwarf,Scalar(.001) * paru); /* Computing MAX */ + if (par == 0.) + par = std::max(dwarf,Scalar(.001) * paru); /* Computing MAX */ - temp = ei_sqrt(par); - wa1 = temp * diag; + temp = ei_sqrt(par); + wa1 = temp * diag; - ipvt.cwise()+=1; // qrsolv() expects the fortran convention (as qrfac provides) - ei_qrsolv(n, r.data(), r.rows(), ipvt.data(), wa1.data(), qtb.data(), x.data(), sdiag.data(), wa2.data()); - ipvt.cwise()-=1; + ipvt.cwise()+=1; // qrsolv() expects the fortran convention (as qrfac provides) + ei_qrsolv(n, r.data(), r.rows(), ipvt.data(), wa1.data(), qtb.data(), x.data(), sdiag.data(), wa2.data()); + ipvt.cwise()-=1; - wa2 = diag.cwise() * x; - dxnorm = wa2.blueNorm(); - temp = fp; - fp = dxnorm - delta; + wa2 = diag.cwise() * x; + dxnorm = wa2.blueNorm(); + 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 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 (ei_abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) + break; + + /* compute the newton correction. */ + + for (j = 0; j < n; ++j) { + l = ipvt[j]; + wa1[j] = diag[l] * (wa2[l] / dxnorm); + /* L180: */ + } + for (j = 0; j < n; ++j) { + wa1[j] /= sdiag[j]; + temp = wa1[j]; + for (i = j+1; i < n; ++i) + wa1[i] -= r(i,j) * temp; + } + temp = wa1.blueNorm(); + parc = fp / delta / temp / temp; + + /* depending on the sign of the function, update parl or paru. */ + + if (fp > 0.) + parl = std::max(parl,par); + if (fp < 0.) + paru = std::min(paru,par); + + /* compute an improved estimate for par. */ + + /* Computing MAX */ + par = std::max(parl,par+parc); + + /* end of an iteration. */ - if (ei_abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || - iter == 10) { - goto L220; } - /* compute the newton correction. */ + /* termination. */ - for (j = 0; j < n; ++j) { - l = ipvt[j]; - wa1[j] = diag[l] * (wa2[l] / dxnorm); - /* L180: */ - } - for (j = 0; j < n; ++j) { - wa1[j] /= sdiag[j]; - temp = wa1[j]; - for (i = j+1; i < n; ++i) - wa1[i] -= r(i,j) * temp; - } - temp = wa1.blueNorm(); - parc = fp / delta / temp / temp; - - /* depending on the sign of the function, update parl or paru. */ - - if (fp > 0.) { - parl = std::max(parl,par); - } - if (fp < 0.) { - paru = std::min(paru,par); - } - - /* compute an improved estimate for par. */ - - /* Computing MAX */ - par = std::max(parl,par+parc); - - /* end of an iteration. */ - - goto L150; -L220: - - /* termination. */ - - if (iter == 0) { + if (iter == 0) par = 0.; - } return; }