merge files
diff --git a/unsupported/Eigen/NonLinear b/unsupported/Eigen/NonLinear
index 4790dfe..8c373ad 100644
--- a/unsupported/Eigen/NonLinear
+++ b/unsupported/Eigen/NonLinear
@@ -50,10 +50,6 @@
#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 0545cfe..7836367 100644
--- a/unsupported/Eigen/src/NonLinear/HybridNonLinearSolver.h
+++ b/unsupported/Eigen/src/NonLinear/HybridNonLinearSolver.h
@@ -20,6 +20,23 @@
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 @@
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 38147f7..175c743 100644
--- a/unsupported/Eigen/src/NonLinear/LevenbergMarquardt.h
+++ b/unsupported/Eigen/src/NonLinear/LevenbergMarquardt.h
@@ -24,6 +24,42 @@
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 @@
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 d7e9134..0000000
--- 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 34d535a..0000000
--- 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 a02224e..0000000
--- 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;
-}
-
diff --git a/unsupported/test/NonLinear.cpp b/unsupported/test/NonLinear.cpp
index b0e8de7..39a5239 100644
--- a/unsupported/test/NonLinear.cpp
+++ b/unsupported/test/NonLinear.cpp
@@ -354,8 +354,8 @@
// do the computation
hybrd_functor functor;
- HybridNonLinearSolverNumericalDiff<hybrd_functor> solver(functor);
- info = solver.solve(x);
+ HybridNonLinearSolver<hybrd_functor> solver(functor);
+ info = solver.solveNumericalDiff(x);
// check return value
VERIFY( 1 == info);
@@ -385,8 +385,8 @@
// do the computation
hybrd_functor functor;
- HybridNonLinearSolverNumericalDiff<hybrd_functor> solver(functor);
- info = solver.solve(x, nfev, diag, mode, ml, mu);
+ HybridNonLinearSolver<hybrd_functor> solver(functor);
+ info = solver.solveNumericalDiff(x, nfev, diag, mode, ml, mu);
// check return value
VERIFY( 1 == info);
@@ -456,8 +456,8 @@
// do the computation
lmstr_functor functor;
- LevenbergMarquardtOptimumStorage<lmstr_functor> lm(functor);
- info = lm.minimize(x);
+ LevenbergMarquardt<lmstr_functor> lm(functor);
+ info = lm.minimizeOptimumStorage(x);
// check return value
VERIFY( 1 == info);
@@ -483,8 +483,8 @@
// do the computation
lmstr_functor functor;
- LevenbergMarquardtOptimumStorage<lmstr_functor> lm(functor);
- info = lm.minimize(x, nfev, njev, diag);
+ LevenbergMarquardt<lmstr_functor> lm(functor);
+ info = lm.minimizeOptimumStorage(x, nfev, njev, diag);
// check return values
VERIFY( 1 == info);
@@ -541,8 +541,8 @@
// do the computation
lmdif_functor functor;
- LevenbergMarquardtNumericalDiff<lmdif_functor> lm(functor);
- info = lm.minimize(x);
+ LevenbergMarquardt<lmdif_functor> lm(functor);
+ info = lm.minimizeNumericalDiff(x);
// check return value
VERIFY( 1 == info);
@@ -569,8 +569,8 @@
// do the computation
lmdif_functor functor;
- LevenbergMarquardtNumericalDiff<lmdif_functor> lm(functor);
- info = lm.minimize(x, nfev, diag);
+ LevenbergMarquardt<lmdif_functor> lm(functor);
+ info = lm.minimizeNumericalDiff(x, nfev, diag);
// check return values
VERIFY( 1 == info);
