diff options
Diffstat (limited to 'unsupported/Eigen')
-rw-r--r-- | unsupported/Eigen/NonLinear | 4 | ||||
-rw-r--r-- | unsupported/Eigen/src/NonLinear/HybridNonLinearSolver.h | 358 | ||||
-rw-r--r-- | unsupported/Eigen/src/NonLinear/LevenbergMarquardt.h | 648 | ||||
-rw-r--r-- | unsupported/Eigen/src/NonLinear/hybrd.h | 374 | ||||
-rw-r--r-- | unsupported/Eigen/src/NonLinear/lmdif.h | 330 | ||||
-rw-r--r-- | unsupported/Eigen/src/NonLinear/lmstr.h | 348 |
6 files changed, 1006 insertions, 1056 deletions
diff --git a/unsupported/Eigen/NonLinear b/unsupported/Eigen/NonLinear index 4790dfe7e..8c373adbf 100644 --- a/unsupported/Eigen/NonLinear +++ b/unsupported/Eigen/NonLinear @@ -50,10 +50,6 @@ namespace Eigen { #include "src/NonLinear/chkder.h" -#include "src/NonLinear/hybrd.h" -#include "src/NonLinear/lmstr.h" -#include "src/NonLinear/lmdif.h" - #include "src/NonLinear/HybridNonLinearSolver.h" #include "src/NonLinear/LevenbergMarquardt.h" diff --git a/unsupported/Eigen/src/NonLinear/HybridNonLinearSolver.h b/unsupported/Eigen/src/NonLinear/HybridNonLinearSolver.h index 0545cfe79..7836367ad 100644 --- a/unsupported/Eigen/src/NonLinear/HybridNonLinearSolver.h +++ b/unsupported/Eigen/src/NonLinear/HybridNonLinearSolver.h @@ -20,6 +20,23 @@ public: const Scalar xtol = ei_sqrt(epsilon<Scalar>()) ); + int solveNumericalDiff( + Matrix< Scalar, Dynamic, 1 > &x, + const Scalar tol = ei_sqrt(epsilon<Scalar>()) + ); + int solveNumericalDiff( + Matrix< Scalar, Dynamic, 1 > &x, + int &nfev, + Matrix< Scalar, Dynamic, 1 > &diag, + const int mode=1, + int nb_of_subdiagonals = -1, + int nb_of_superdiagonals = -1, + const int maxfev = 2000, + const Scalar factor = Scalar(100.), + const Scalar xtol = ei_sqrt(epsilon<Scalar>()), + const Scalar epsfcn = Scalar(0.) + ); + Matrix< Scalar, Dynamic, 1 > fvec; Matrix< Scalar, Dynamic, Dynamic > fjac; Matrix< Scalar, Dynamic, 1 > R; @@ -357,3 +374,344 @@ algo_end: return info; } + + +template<typename FunctorType, typename Scalar> +int HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff( + Matrix< Scalar, Dynamic, 1 > &x, + const Scalar tol + ) +{ + const int n = x.size(); + int info, nfev=0; + Matrix< Scalar, Dynamic, 1> diag; + + /* check the input parameters for errors. */ + if (n <= 0 || tol < 0.) { + printf("HybridNonLinearSolver::solve() bad args : n,tol,..."); + return 0; + } + + diag.setConstant(n, 1.); + info = solveNumericalDiff( + x, + nfev, + diag, + 2, + -1, -1, + (n+1)*200, + 100., + tol, Scalar(0.) + ); + return (info==5)?4:info; +} + + +template<typename FunctorType, typename Scalar> +int HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff( + Matrix< Scalar, Dynamic, 1 > &x, + int &nfev, + Matrix< Scalar, Dynamic, 1 > &diag, + const int mode, + int nb_of_subdiagonals, + int nb_of_superdiagonals, + const int maxfev, + const Scalar factor, + const Scalar xtol, + const Scalar epsfcn + ) +{ + const int n = x.size(); + Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n), wa3(n), wa4(n); + + + if (nb_of_subdiagonals<0) nb_of_subdiagonals = n-1; + if (nb_of_superdiagonals<0) nb_of_superdiagonals = n-1; + qtf.resize(n); + R.resize( (n*(n+1))/2); + fjac.resize(n, n); + fvec.resize(n); + + /* Local variables */ + int i, j, l, iwa[1]; + Scalar sum; + int sing; + int iter; + Scalar temp; + int msum, iflag; + Scalar delta; + int jeval; + int ncsuc; + Scalar ratio; + Scalar fnorm; + Scalar pnorm, xnorm, fnorm1; + int nslow1, nslow2; + int ncfail; + Scalar actred, prered; + int info; + + /* Function Body */ + + info = 0; + iflag = 0; + nfev = 0; + + /* check the input parameters for errors. */ + + if (n <= 0 || xtol < 0. || maxfev <= 0 || nb_of_subdiagonals < 0 || nb_of_superdiagonals < 0 || factor <= 0. ) + goto algo_end; + if (mode == 2) + for (j = 0; j < n; ++j) + if (diag[j] <= 0.) goto algo_end; + + /* evaluate the function at the starting point */ + /* and calculate its norm. */ + + iflag = functor.f(x, fvec); + nfev = 1; + if (iflag < 0) + goto algo_end; + fnorm = fvec.stableNorm(); + + /* determine the number of calls to fcn needed to compute */ + /* the jacobian matrix. */ + + /* Computing MIN */ + msum = std::min(nb_of_subdiagonals + nb_of_superdiagonals + 1, n); + + /* initialize iteration counter and monitors. */ + + iter = 1; + ncsuc = 0; + ncfail = 0; + nslow1 = 0; + nslow2 = 0; + + /* beginning of the outer loop. */ + + while (true) { + jeval = true; + + /* calculate the jacobian matrix. */ + + iflag = ei_fdjac1(functor, x, fvec, fjac, + nb_of_subdiagonals, nb_of_superdiagonals, epsfcn); + nfev += msum; + if (iflag < 0) + break; + + /* compute the qr factorization of the jacobian. */ + + ei_qrfac<Scalar>(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. */ + + 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; + } + + /* form (q transpose)*fvec and store in qtf. */ + + qtf = fvec; + 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; + } + + /* copy the triangular factor of the qr factorization into r. */ + + sing = false; + for (j = 0; j < n; ++j) { + l = j; + if (j) + for (i = 0; i < j; ++i) { + R[l] = fjac(i,j); + l = l + n - i -1; + } + R[l] = wa1[j]; + if (wa1[j] == 0.) + sing = true; + } + + /* accumulate the orthogonal factor in fjac. */ + + ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data()); + + /* rescale if necessary. */ + + /* Computing MAX */ + if (mode != 2) + diag = diag.cwise().max(wa2); + + /* beginning of the inner loop. */ + + while (true) { + + /* determine the direction p. */ + + ei_dogleg<Scalar>(R, diag, qtf, delta, wa1); + + /* 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 algo_end; + fnorm1 = wa4.stableNorm(); + + /* compute the scaled actual reduction. */ + + actred = -1.; + if (fnorm1 < fnorm) /* Computing 2nd power */ + actred = 1. - ei_abs2(fnorm1 / fnorm); + + /* compute the scaled predicted reduction. */ + + l = 0; + for (i = 0; i < n; ++i) { + sum = 0.; + for (j = i; j < n; ++j) { + sum += R[l] * wa1[j]; + ++l; + } + wa3[i] = qtf[i] + sum; + } + temp = wa3.stableNorm(); + prered = 0.; + if (temp < fnorm) /* Computing 2nd power */ + prered = 1. - ei_abs2(temp / fnorm); + + /* 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(.1)) { + ncsuc = 0; + ++ncfail; + delta = Scalar(.5) * delta; + } else { + ncfail = 0; + ++ncsuc; + if (ratio >= Scalar(.5) || ncsuc > 1) /* Computing MAX */ + delta = std::max(delta, pnorm / Scalar(.5)); + if (ei_abs(ratio - 1.) <= Scalar(.1)) { + delta = pnorm / Scalar(.5); + } + } + + /* 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; + } + + /* determine the progress of the iteration. */ + + ++nslow1; + if (actred >= Scalar(.001)) + nslow1 = 0; + if (jeval) + ++nslow2; + if (actred >= Scalar(.1)) + nslow2 = 0; + + /* test for convergence. */ + + if (delta <= xtol * xnorm || fnorm == 0.) + info = 1; + if (info != 0) + goto algo_end; + + /* tests for termination and stringent tolerances. */ + + if (nfev >= maxfev) + info = 2; + /* Computing MAX */ + if (Scalar(.1) * std::max(Scalar(.1) * delta, pnorm) <= epsilon<Scalar>() * xnorm) + info = 3; + if (nslow2 == 5) + info = 4; + if (nslow1 == 10) + info = 5; + if (info != 0) + goto algo_end; + + /* 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. */ + + for (j = 0; j < n; ++j) { + sum = wa4.dot(fjac.col(j)); + wa2[j] = (sum - wa3[j]) / pnorm; + wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm); + if (ratio >= Scalar(1e-4)) + qtf[j] = sum; + } + + /* compute the qr factorization of the updated jacobian. */ + + ei_r1updt<Scalar>(n, n, R.data(), R.size(), wa1.data(), wa2.data(), wa3.data(), &sing); + ei_r1mpyq<Scalar>(n, n, fjac.data(), fjac.rows(), wa2.data(), wa3.data()); + ei_r1mpyq<Scalar>(1, n, qtf.data(), 1, wa2.data(), wa3.data()); + + /* end of the inner loop. */ + + jeval = false; + } + /* end of the outer loop. */ + } +algo_end: + /* termination, either normal or user imposed. */ + if (iflag < 0) + info = iflag; + return info; +} + diff --git a/unsupported/Eigen/src/NonLinear/LevenbergMarquardt.h b/unsupported/Eigen/src/NonLinear/LevenbergMarquardt.h index 38147f7ae..175c74395 100644 --- a/unsupported/Eigen/src/NonLinear/LevenbergMarquardt.h +++ b/unsupported/Eigen/src/NonLinear/LevenbergMarquardt.h @@ -24,6 +24,42 @@ public: const Scalar gtol = Scalar(0.) ); + int minimizeNumericalDiff( + Matrix< Scalar, Dynamic, 1 > &x, + const Scalar tol = ei_sqrt(epsilon<Scalar>()) + ); + + int minimizeNumericalDiff( + Matrix< Scalar, Dynamic, 1 > &x, + int &nfev, + Matrix< Scalar, Dynamic, 1 > &diag, + const int mode=1, + const Scalar factor = Scalar(100.), + const int maxfev = 400, + const Scalar ftol = ei_sqrt(epsilon<Scalar>()), + const Scalar xtol = ei_sqrt(epsilon<Scalar>()), + const Scalar gtol = Scalar(0.), + const Scalar epsfcn = Scalar(0.) + ); + + int minimizeOptimumStorage( + Matrix< Scalar, Dynamic, 1 > &x, + const Scalar tol = ei_sqrt(epsilon<Scalar>()) + ); + + int minimizeOptimumStorage( + Matrix< Scalar, Dynamic, 1 > &x, + int &nfev, + int &njev, + Matrix< Scalar, Dynamic, 1 > &diag, + const int mode=1, + const Scalar factor = Scalar(100.), + const int maxfev = 400, + const Scalar ftol = ei_sqrt(epsilon<Scalar>()), + const Scalar xtol = ei_sqrt(epsilon<Scalar>()), + const Scalar gtol = Scalar(0.) + ); + Matrix< Scalar, Dynamic, 1 > fvec; Matrix< Scalar, Dynamic, Dynamic > fjac; VectorXi ipvt; @@ -330,3 +366,615 @@ algo_end: info = iflag; return info; } + + +template<typename FunctorType, typename Scalar> +int LevenbergMarquardt<FunctorType,Scalar>::minimizeNumericalDiff( + Matrix< Scalar, Dynamic, 1 > &x, + const Scalar tol + ) +{ + const int n = x.size(); + const int m = functor.nbOfFunctions(); + int info, nfev=0; + Matrix< Scalar, Dynamic, Dynamic > fjac(m, n); + Matrix< Scalar, Dynamic, 1> diag, qtf; + VectorXi ipvt; + + /* check the input parameters for errors. */ + if (n <= 0 || m < n || tol < 0.) { + printf("ei_lmder1 bad args : m,n,tol,..."); + return 0; + } + + info = minimizeNumericalDiff( + x, + nfev, + diag, + 1, + 100., + (n+1)*200, + tol, tol, Scalar(0.), Scalar(0.) + ); + return (info==8)?4:info; +} + +template<typename FunctorType, typename Scalar> +int LevenbergMarquardt<FunctorType,Scalar>::minimizeNumericalDiff( + Matrix< Scalar, Dynamic, 1 > &x, + int &nfev, + Matrix< Scalar, Dynamic, 1 > &diag, + const int mode, + const Scalar factor, + const int maxfev, + const Scalar ftol, + const Scalar xtol, + const Scalar gtol, + const Scalar epsfcn + ) +{ + const int n = x.size(); + const int m = functor.nbOfFunctions(); + Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n), wa3(n), wa4(m); + + fvec.resize(m); + ipvt.resize(n); + fjac.resize(m, n); + diag.resize(n); + qtf.resize(n); + + /* Local variables */ + int i, j, l; + Scalar par, sum; + int iter; + Scalar temp, temp1, temp2; + int iflag; + Scalar delta; + Scalar ratio; + Scalar fnorm, gnorm; + Scalar pnorm, xnorm, fnorm1, actred, dirder, prered; + int info; + + /* Function Body */ + info = 0; + iflag = 0; + nfev = 0; + + /* check the input parameters for errors. */ + + if (n <= 0 || m < n || ftol < 0. || xtol < 0. || gtol < 0. || maxfev <= 0 || factor <= 0.) + goto algo_end; + if (mode == 2) + for (j = 0; j < n; ++j) + if (diag[j] <= 0.) goto algo_end; + + /* evaluate the function at the starting point */ + /* and calculate its norm. */ + + iflag = functor.f(x, fvec); + nfev = 1; + if (iflag < 0) + goto algo_end; + fnorm = fvec.stableNorm(); + + /* initialize levenberg-marquardt parameter and iteration counter. */ + + par = 0.; + iter = 1; + + /* beginning of the outer loop. */ + + while (true) { + + /* calculate the jacobian matrix. */ + + iflag = ei_fdjac2(functor, x, fvec, fjac, epsfcn); + nfev += n; + if (iflag < 0) + break; + + /* compute the qr factorization of the jacobian. */ + + ei_qrfac<Scalar>(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) { + 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; + } + + /* 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]; + } + + /* 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<Scalar>(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 algo_end; + 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 algo_end; + + /* tests for termination and stringent tolerances. */ + + if (nfev >= maxfev) + info = 5; + if (ei_abs(actred) <= epsilon<Scalar>() && prered <= epsilon<Scalar>() && Scalar(.5) * ratio <= 1.) + info = 6; + if (delta <= epsilon<Scalar>() * xnorm) + info = 7; + if (gnorm <= epsilon<Scalar>()) + info = 8; + if (info != 0) + goto algo_end; + /* end of the inner loop. repeat if iteration unsuccessful. */ + } while (ratio < Scalar(1e-4)); + /* end of the outer loop. */ + } +algo_end: + + /* termination, either normal or user imposed. */ + if (iflag < 0) + info = iflag; + return info; +} + + +template<typename FunctorType, typename Scalar> +int LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorage( + Matrix< Scalar, Dynamic, 1 > &x, + const Scalar tol + ) +{ + const int n = x.size(); + const int m = functor.nbOfFunctions(); + int info, nfev=0, njev=0; + Matrix< Scalar, Dynamic, Dynamic > fjac(m, n); + Matrix< Scalar, Dynamic, 1> diag, qtf; + VectorXi ipvt; + + /* check the input parameters for errors. */ + if (n <= 0 || m < n || tol < 0.) { + printf("LevenbergMarquardtOptimumStorage::minimize() bad args : m,n,tol,..."); + return 0; + } + + info = minimizeOptimumStorage( + x, + nfev, njev, + diag, + 1, + 100., + (n+1)*100, + tol, tol, Scalar(0.) + ); + return (info==8)?4:info; +} + +template<typename FunctorType, typename Scalar> +int LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorage( + Matrix< Scalar, Dynamic, 1 > &x, + int &nfev, + int &njev, + Matrix< Scalar, Dynamic, 1 > &diag, + const int mode, + const Scalar factor, + const int maxfev, + const Scalar ftol, + const Scalar xtol, + const Scalar gtol + ) +{ + const int n = x.size(); + const int m = functor.nbOfFunctions(); + Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n), wa3(n), wa4(m); + + fvec.resize(m); + ipvt.resize(n); + fjac.resize(m, n); + diag.resize(n); + qtf.resize(n); + + /* Local variables */ + int i, j, l; + Scalar par, sum; + int sing, iter; + Scalar temp, temp1, temp2; + int iflag; + Scalar delta; + Scalar ratio; + Scalar fnorm, gnorm; + Scalar pnorm, xnorm, fnorm1, actred, dirder, prered; + int info; + + /* Function Body */ + info = 0; + iflag = 0; + nfev = 0; + njev = 0; + + /* check the input parameters for errors. */ + + if (n <= 0 || m < n || ftol < 0. || xtol < 0. || gtol < 0. || maxfev <= 0 || factor <= 0.) + goto algo_end; + + if (mode == 2) + for (j = 0; j < n; ++j) + if (diag[j] <= 0.) goto algo_end; + + /* evaluate the function at the starting point */ + /* and calculate its norm. */ + + iflag = functor.f(x, fvec); + nfev = 1; + if (iflag < 0) + goto algo_end; + fnorm = fvec.stableNorm(); + + /* initialize levenberg-marquardt parameter and iteration counter. */ + + par = 0.; + iter = 1; + + /* beginning of the outer loop. */ + + while (true) { + + /* 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<Scalar>(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; + } + ipvt[j] = j; + wa2[j] = fjac.col(j).start(j).stableNorm(); + } + if (sing) { + ipvt.cwise()+=1; + ei_qrfac<Scalar>(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]; + } + } + + /* 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. */ + + 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<Scalar>(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 algo_end; + 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 algo_end; + + /* tests for termination and stringent tolerances. */ + + if (nfev >= maxfev) + info = 5; + if (ei_abs(actred) <= epsilon<Scalar>() && prered <= epsilon<Scalar>() && Scalar(.5) * ratio <= 1.) + info = 6; + if (delta <= epsilon<Scalar>() * xnorm) + info = 7; + if (gnorm <= epsilon<Scalar>()) + info = 8; + if (info != 0) + goto algo_end; + /* end of the inner loop. repeat if iteration unsuccessful. */ + } while (ratio < Scalar(1e-4)); + /* end of the outer loop. */ + } +algo_end: + + /* termination, either normal or user imposed. */ + if (iflag < 0) + info = iflag; + return info; +} + + diff --git a/unsupported/Eigen/src/NonLinear/hybrd.h b/unsupported/Eigen/src/NonLinear/hybrd.h deleted file mode 100644 index d7e913414..000000000 --- a/unsupported/Eigen/src/NonLinear/hybrd.h +++ /dev/null @@ -1,374 +0,0 @@ - -template<typename FunctorType, typename Scalar=double> -class HybridNonLinearSolverNumericalDiff -{ -public: - HybridNonLinearSolverNumericalDiff(const FunctorType &_functor) - : functor(_functor) {} - - int solve( - Matrix< Scalar, Dynamic, 1 > &x, - const Scalar tol = ei_sqrt(epsilon<Scalar>()) - ); - int solve( - Matrix< Scalar, Dynamic, 1 > &x, - int &nfev, - Matrix< Scalar, Dynamic, 1 > &diag, - const int mode=1, - int nb_of_subdiagonals = -1, - int nb_of_superdiagonals = -1, - const int maxfev = 2000, - const Scalar factor = Scalar(100.), - const Scalar xtol = ei_sqrt(epsilon<Scalar>()), - const Scalar epsfcn = Scalar(0.) - ); - - Matrix< Scalar, Dynamic, 1 > fvec; - Matrix< Scalar, Dynamic, Dynamic > fjac; - Matrix< Scalar, Dynamic, 1 > R; - Matrix< Scalar, Dynamic, 1 > qtf; -private: - const FunctorType &functor; -}; - - - -template<typename FunctorType, typename Scalar> -int HybridNonLinearSolverNumericalDiff<FunctorType,Scalar>::solve( - Matrix< Scalar, Dynamic, 1 > &x, - const Scalar tol - ) -{ - const int n = x.size(); - int info, nfev=0; - Matrix< Scalar, Dynamic, 1> diag; - - /* check the input parameters for errors. */ - if (n <= 0 || tol < 0.) { - printf("HybridNonLinearSolver::solve() bad args : n,tol,..."); - return 0; - } - - diag.setConstant(n, 1.); - info = solve( - x, - nfev, - diag, - 2, - -1, -1, - (n+1)*200, - 100., - tol, Scalar(0.) - ); - return (info==5)?4:info; -} - - -template<typename FunctorType, typename Scalar> -int HybridNonLinearSolverNumericalDiff<FunctorType,Scalar>::solve( - Matrix< Scalar, Dynamic, 1 > &x, - int &nfev, - Matrix< Scalar, Dynamic, 1 > &diag, - const int mode, - int nb_of_subdiagonals, - int nb_of_superdiagonals, - const int maxfev, - const Scalar factor, - const Scalar xtol, - const Scalar epsfcn - ) -{ - const int n = x.size(); - Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n), wa3(n), wa4(n); - - - if (nb_of_subdiagonals<0) nb_of_subdiagonals = n-1; - if (nb_of_superdiagonals<0) nb_of_superdiagonals = n-1; - qtf.resize(n); - R.resize( (n*(n+1))/2); - fjac.resize(n, n); - fvec.resize(n); - - /* Local variables */ - int i, j, l, iwa[1]; - Scalar sum; - int sing; - int iter; - Scalar temp; - int msum, iflag; - Scalar delta; - int jeval; - int ncsuc; - Scalar ratio; - Scalar fnorm; - Scalar pnorm, xnorm, fnorm1; - int nslow1, nslow2; - int ncfail; - Scalar actred, prered; - int info; - - /* Function Body */ - - info = 0; - iflag = 0; - nfev = 0; - - /* check the input parameters for errors. */ - - if (n <= 0 || xtol < 0. || maxfev <= 0 || nb_of_subdiagonals < 0 || nb_of_superdiagonals < 0 || factor <= 0. ) - goto algo_end; - if (mode == 2) - for (j = 0; j < n; ++j) - if (diag[j] <= 0.) goto algo_end; - - /* evaluate the function at the starting point */ - /* and calculate its norm. */ - - iflag = functor.f(x, fvec); - nfev = 1; - if (iflag < 0) - goto algo_end; - fnorm = fvec.stableNorm(); - - /* determine the number of calls to fcn needed to compute */ - /* the jacobian matrix. */ - - /* Computing MIN */ - msum = std::min(nb_of_subdiagonals + nb_of_superdiagonals + 1, n); - - /* initialize iteration counter and monitors. */ - - iter = 1; - ncsuc = 0; - ncfail = 0; - nslow1 = 0; - nslow2 = 0; - - /* beginning of the outer loop. */ - - while (true) { - jeval = true; - - /* calculate the jacobian matrix. */ - - iflag = ei_fdjac1(functor, x, fvec, fjac, - nb_of_subdiagonals, nb_of_superdiagonals, epsfcn); - nfev += msum; - if (iflag < 0) - break; - - /* compute the qr factorization of the jacobian. */ - - ei_qrfac<Scalar>(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. */ - - 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; - } - - /* form (q transpose)*fvec and store in qtf. */ - - qtf = fvec; - 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; - } - - /* copy the triangular factor of the qr factorization into r. */ - - sing = false; - for (j = 0; j < n; ++j) { - l = j; - if (j) - for (i = 0; i < j; ++i) { - R[l] = fjac(i,j); - l = l + n - i -1; - } - R[l] = wa1[j]; - if (wa1[j] == 0.) - sing = true; - } - - /* accumulate the orthogonal factor in fjac. */ - - ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data()); - - /* rescale if necessary. */ - - /* Computing MAX */ - if (mode != 2) - diag = diag.cwise().max(wa2); - - /* beginning of the inner loop. */ - - while (true) { - - /* determine the direction p. */ - - ei_dogleg<Scalar>(R, diag, qtf, delta, wa1); - - /* 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 algo_end; - fnorm1 = wa4.stableNorm(); - - /* compute the scaled actual reduction. */ - - actred = -1.; - if (fnorm1 < fnorm) /* Computing 2nd power */ - actred = 1. - ei_abs2(fnorm1 / fnorm); - - /* compute the scaled predicted reduction. */ - - l = 0; - for (i = 0; i < n; ++i) { - sum = 0.; - for (j = i; j < n; ++j) { - sum += R[l] * wa1[j]; - ++l; - } - wa3[i] = qtf[i] + sum; - } - temp = wa3.stableNorm(); - prered = 0.; - if (temp < fnorm) /* Computing 2nd power */ - prered = 1. - ei_abs2(temp / fnorm); - - /* 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(.1)) { - ncsuc = 0; - ++ncfail; - delta = Scalar(.5) * delta; - } else { - ncfail = 0; - ++ncsuc; - if (ratio >= Scalar(.5) || ncsuc > 1) /* Computing MAX */ - delta = std::max(delta, pnorm / Scalar(.5)); - if (ei_abs(ratio - 1.) <= Scalar(.1)) { - delta = pnorm / Scalar(.5); - } - } - - /* 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; - } - - /* determine the progress of the iteration. */ - - ++nslow1; - if (actred >= Scalar(.001)) - nslow1 = 0; - if (jeval) - ++nslow2; - if (actred >= Scalar(.1)) - nslow2 = 0; - - /* test for convergence. */ - - if (delta <= xtol * xnorm || fnorm == 0.) - info = 1; - if (info != 0) - goto algo_end; - - /* tests for termination and stringent tolerances. */ - - if (nfev >= maxfev) - info = 2; - /* Computing MAX */ - if (Scalar(.1) * std::max(Scalar(.1) * delta, pnorm) <= epsilon<Scalar>() * xnorm) - info = 3; - if (nslow2 == 5) - info = 4; - if (nslow1 == 10) - info = 5; - if (info != 0) - goto algo_end; - - /* 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. */ - - for (j = 0; j < n; ++j) { - sum = wa4.dot(fjac.col(j)); - wa2[j] = (sum - wa3[j]) / pnorm; - wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm); - if (ratio >= Scalar(1e-4)) - qtf[j] = sum; - } - - /* compute the qr factorization of the updated jacobian. */ - - ei_r1updt<Scalar>(n, n, R.data(), R.size(), wa1.data(), wa2.data(), wa3.data(), &sing); - ei_r1mpyq<Scalar>(n, n, fjac.data(), fjac.rows(), wa2.data(), wa3.data()); - ei_r1mpyq<Scalar>(1, n, qtf.data(), 1, wa2.data(), wa3.data()); - - /* end of the inner loop. */ - - jeval = false; - } - /* end of the outer loop. */ - } -algo_end: - /* termination, either normal or user imposed. */ - if (iflag < 0) - info = iflag; - return info; -} - diff --git a/unsupported/Eigen/src/NonLinear/lmdif.h b/unsupported/Eigen/src/NonLinear/lmdif.h deleted file mode 100644 index 34d535add..000000000 --- a/unsupported/Eigen/src/NonLinear/lmdif.h +++ /dev/null @@ -1,330 +0,0 @@ - -template<typename FunctorType, typename Scalar=double> -class LevenbergMarquardtNumericalDiff -{ -public: - LevenbergMarquardtNumericalDiff(const FunctorType &_functor) - : functor(_functor) {} - - int minimize( - Matrix< Scalar, Dynamic, 1 > &x, - const Scalar tol = ei_sqrt(epsilon<Scalar>()) - ); - - int minimize( - Matrix< Scalar, Dynamic, 1 > &x, - int &nfev, - Matrix< Scalar, Dynamic, 1 > &diag, - const int mode=1, - const Scalar factor = Scalar(100.), - const int maxfev = 400, - const Scalar ftol = ei_sqrt(epsilon<Scalar>()), - const Scalar xtol = ei_sqrt(epsilon<Scalar>()), - const Scalar gtol = Scalar(0.), - const Scalar epsfcn = Scalar(0.) - ); - - Matrix< Scalar, Dynamic, 1 > fvec; - Matrix< Scalar, Dynamic, Dynamic > fjac; - VectorXi ipvt; - Matrix< Scalar, Dynamic, 1 > qtf; -private: - const FunctorType &functor; -}; - - -template<typename FunctorType, typename Scalar> -int LevenbergMarquardtNumericalDiff<FunctorType,Scalar>::minimize( - Matrix< Scalar, Dynamic, 1 > &x, - const Scalar tol - ) -{ - const int n = x.size(); - const int m = functor.nbOfFunctions(); - int info, nfev=0; - Matrix< Scalar, Dynamic, Dynamic > fjac(m, n); - Matrix< Scalar, Dynamic, 1> diag, qtf; - VectorXi ipvt; - - /* check the input parameters for errors. */ - if (n <= 0 || m < n || tol < 0.) { - printf("ei_lmder1 bad args : m,n,tol,..."); - return 0; - } - - info = minimize( - x, - nfev, - diag, - 1, - 100., - (n+1)*200, - tol, tol, Scalar(0.), Scalar(0.) - ); - return (info==8)?4:info; -} - -template<typename FunctorType, typename Scalar> -int LevenbergMarquardtNumericalDiff<FunctorType,Scalar>::minimize( - Matrix< Scalar, Dynamic, 1 > &x, - int &nfev, - Matrix< Scalar, Dynamic, 1 > &diag, - const int mode, - const Scalar factor, - const int maxfev, - const Scalar ftol, - const Scalar xtol, - const Scalar gtol, - const Scalar epsfcn - ) -{ - const int n = x.size(); - const int m = functor.nbOfFunctions(); - Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n), wa3(n), wa4(m); - - fvec.resize(m); - ipvt.resize(n); - fjac.resize(m, n); - diag.resize(n); - qtf.resize(n); - - /* Local variables */ - int i, j, l; - Scalar par, sum; - int iter; - Scalar temp, temp1, temp2; - int iflag; - Scalar delta; - Scalar ratio; - Scalar fnorm, gnorm; - Scalar pnorm, xnorm, fnorm1, actred, dirder, prered; - int info; - - /* Function Body */ - info = 0; - iflag = 0; - nfev = 0; - - /* check the input parameters for errors. */ - - if (n <= 0 || m < n || ftol < 0. || xtol < 0. || gtol < 0. || maxfev <= 0 || factor <= 0.) - goto algo_end; - if (mode == 2) - for (j = 0; j < n; ++j) - if (diag[j] <= 0.) goto algo_end; - - /* evaluate the function at the starting point */ - /* and calculate its norm. */ - - iflag = functor.f(x, fvec); - nfev = 1; - if (iflag < 0) - goto algo_end; - fnorm = fvec.stableNorm(); - - /* initialize levenberg-marquardt parameter and iteration counter. */ - - par = 0.; - iter = 1; - - /* beginning of the outer loop. */ - - while (true) { - - /* calculate the jacobian matrix. */ - - iflag = ei_fdjac2(functor, x, fvec, fjac, epsfcn); - nfev += n; - if (iflag < 0) - break; - - /* compute the qr factorization of the jacobian. */ - - ei_qrfac<Scalar>(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) { - 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; - } - - /* 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]; - } - - /* 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<Scalar>(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 algo_end; - 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 algo_end; - - /* tests for termination and stringent tolerances. */ - - if (nfev >= maxfev) - info = 5; - if (ei_abs(actred) <= epsilon<Scalar>() && prered <= epsilon<Scalar>() && Scalar(.5) * ratio <= 1.) - info = 6; - if (delta <= epsilon<Scalar>() * xnorm) - info = 7; - if (gnorm <= epsilon<Scalar>()) - info = 8; - if (info != 0) - goto algo_end; - /* end of the inner loop. repeat if iteration unsuccessful. */ - } while (ratio < Scalar(1e-4)); - /* end of the outer loop. */ - } -algo_end: - - /* termination, either normal or user imposed. */ - if (iflag < 0) - info = iflag; - return info; -} - diff --git a/unsupported/Eigen/src/NonLinear/lmstr.h b/unsupported/Eigen/src/NonLinear/lmstr.h deleted file mode 100644 index a02224e32..000000000 --- a/unsupported/Eigen/src/NonLinear/lmstr.h +++ /dev/null @@ -1,348 +0,0 @@ - -template<typename FunctorType, typename Scalar=double> -class LevenbergMarquardtOptimumStorage -{ -public: - LevenbergMarquardtOptimumStorage(const FunctorType &_functor) - : functor(_functor) {} - - int minimize( - Matrix< Scalar, Dynamic, 1 > &x, - const Scalar tol = ei_sqrt(epsilon<Scalar>()) - ); - - int minimize( - Matrix< Scalar, Dynamic, 1 > &x, - int &nfev, - int &njev, - Matrix< Scalar, Dynamic, 1 > &diag, - const int mode=1, - const Scalar factor = Scalar(100.), - const int maxfev = 400, - const Scalar ftol = ei_sqrt(epsilon<Scalar>()), - const Scalar xtol = ei_sqrt(epsilon<Scalar>()), - const Scalar gtol = Scalar(0.) - ); - - Matrix< Scalar, Dynamic, 1 > fvec; - Matrix< Scalar, Dynamic, Dynamic > fjac; - VectorXi ipvt; - Matrix< Scalar, Dynamic, 1 > qtf; -private: - const FunctorType &functor; -}; - - -template<typename FunctorType, typename Scalar> -int LevenbergMarquardtOptimumStorage<FunctorType,Scalar>::minimize( - Matrix< Scalar, Dynamic, 1 > &x, - const Scalar tol - ) -{ - const int n = x.size(); - const int m = functor.nbOfFunctions(); - int info, nfev=0, njev=0; - Matrix< Scalar, Dynamic, Dynamic > fjac(m, n); - Matrix< Scalar, Dynamic, 1> diag, qtf; - VectorXi ipvt; - - /* check the input parameters for errors. */ - if (n <= 0 || m < n || tol < 0.) { - printf("LevenbergMarquardtOptimumStorage::minimize() bad args : m,n,tol,..."); - return 0; - } - - info = minimize( - x, - nfev, njev, - diag, - 1, - 100., - (n+1)*100, - tol, tol, Scalar(0.) - ); - return (info==8)?4:info; -} - -template<typename FunctorType, typename Scalar> -int LevenbergMarquardtOptimumStorage<FunctorType,Scalar>::minimize( - Matrix< Scalar, Dynamic, 1 > &x, - int &nfev, - int &njev, - Matrix< Scalar, Dynamic, 1 > &diag, - const int mode, - const Scalar factor, - const int maxfev, - const Scalar ftol, - const Scalar xtol, - const Scalar gtol - ) -{ - const int n = x.size(); - const int m = functor.nbOfFunctions(); - Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n), wa3(n), wa4(m); - - fvec.resize(m); - ipvt.resize(n); - fjac.resize(m, n); - diag.resize(n); - qtf.resize(n); - - /* Local variables */ - int i, j, l; - Scalar par, sum; - int sing, iter; - Scalar temp, temp1, temp2; - int iflag; - Scalar delta; - Scalar ratio; - Scalar fnorm, gnorm; - Scalar pnorm, xnorm, fnorm1, actred, dirder, prered; - int info; - - /* Function Body */ - info = 0; - iflag = 0; - nfev = 0; - njev = 0; - - /* check the input parameters for errors. */ - - if (n <= 0 || m < n || ftol < 0. || xtol < 0. || gtol < 0. || maxfev <= 0 || factor <= 0.) - goto algo_end; - - if (mode == 2) - for (j = 0; j < n; ++j) - if (diag[j] <= 0.) goto algo_end; - - /* evaluate the function at the starting point */ - /* and calculate its norm. */ - - iflag = functor.f(x, fvec); - nfev = 1; - if (iflag < 0) - goto algo_end; - fnorm = fvec.stableNorm(); - - /* initialize levenberg-marquardt parameter and iteration counter. */ - - par = 0.; - iter = 1; - - /* beginning of the outer loop. */ - - while (true) { - - /* 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<Scalar>(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; - } - ipvt[j] = j; - wa2[j] = fjac.col(j).start(j).stableNorm(); - } - if (sing) { - ipvt.cwise()+=1; - ei_qrfac<Scalar>(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]; - } - } - - /* 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. */ - - 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<Scalar>(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 algo_end; - 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 algo_end; - - /* tests for termination and stringent tolerances. */ - - if (nfev >= maxfev) - info = 5; - if (ei_abs(actred) <= epsilon<Scalar>() && prered <= epsilon<Scalar>() && Scalar(.5) * ratio <= 1.) - info = 6; - if (delta <= epsilon<Scalar>() * xnorm) - info = 7; - if (gnorm <= epsilon<Scalar>()) - info = 8; - if (info != 0) - goto algo_end; - /* end of the inner loop. repeat if iteration unsuccessful. */ - } while (ratio < Scalar(1e-4)); - /* end of the outer loop. */ - } -algo_end: - - /* termination, either normal or user imposed. */ - if (iflag < 0) - info = iflag; - return info; -} - |