diff --git a/unsupported/Eigen/src/NonLinear/MathFunctions.h b/unsupported/Eigen/src/NonLinear/MathFunctions.h index b79bfbaa2..5d0c5efa5 100644 --- a/unsupported/Eigen/src/NonLinear/MathFunctions.h +++ b/unsupported/Eigen/src/NonLinear/MathFunctions.h @@ -36,6 +36,7 @@ int ei_hybrd1( { int lwa = (x.size()*(3*x.size()+13))/2; Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > wa(lwa); + fvec.resize(x.size()); return hybrd1(Functor::f, 0, x.size(), x.data(), fvec.data(), tol, wa.data(), lwa); } @@ -44,14 +45,16 @@ template int ei_lmder1( Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &x, Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &fvec, + VectorXi &ipvt, Scalar tol = Eigen::ei_sqrt(Eigen::machine_epsilon()) ) { int lwa = 5*x.size()+fvec.size(); - Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > wa(lwa); - VectorXi ipvt(x.size()); int ldfjac = fvec.size(); + Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > wa(lwa); Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > fjac(ldfjac, x.size()); + + ipvt.resize(x.size()); return lmder1 ( Functor::f, 0, fvec.size(), x.size(), x.data(), fvec.data(), @@ -62,5 +65,50 @@ int ei_lmder1( ); } +template +int ei_lmder( + Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &x, + Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &fvec, + int &nfev, + int &njev, + Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > &fjac, + VectorXi &ipvt, + Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &wa1, + Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &diag, + int mode=1, + double factor = 100., + int maxfev = 400, + Scalar ftol = Eigen::ei_sqrt(Eigen::machine_epsilon()), + Scalar xtol = Eigen::ei_sqrt(Eigen::machine_epsilon()), + Scalar gtol = Scalar(0.), + int nprint=0 + ) +{ + Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > + qtf(x.size()), + wa2(x.size()), wa3(x.size()), + wa4(fvec.size()); + int ldfjac = fvec.size(); + + ipvt.resize(x.size()); + wa1.resize(x.size()); + fjac.resize(ldfjac, x.size()); + diag.resize(x.size()); + return lmder ( + Functor::f, 0, + fvec.size(), x.size(), x.data(), fvec.data(), + fjac.data() , ldfjac, + ftol, xtol, gtol, + maxfev, + diag.data(), mode, + factor, + nprint, + &nfev, &njev, + ipvt.data(), + qtf.data(), + wa1.data(), wa2.data(), wa3.data(), wa4.data() + ); +} + #endif // EIGEN_NONLINEAR_MATHFUNCTIONS_H diff --git a/unsupported/test/NonLinear.cpp b/unsupported/test/NonLinear.cpp index 113c66da5..9279875f3 100644 --- a/unsupported/test/NonLinear.cpp +++ b/unsupported/test/NonLinear.cpp @@ -159,12 +159,13 @@ void testLmder1() Eigen::VectorXd x(n), fvec(m); Eigen::MatrixXd fjac(m, n); + VectorXi ipvt; /* the following starting values provide a rough fit. */ x.setConstant(n, 1.); // do the computation - info = ei_lmder1(x, fvec); + info = ei_lmder1(x, fvec, ipvt); // check return value VERIFY( 1 == info); @@ -178,112 +179,101 @@ void testLmder1() VERIFY_IS_APPROX(x, x_ref); } -int fcn_lmder(void * /*p*/, int /*m*/, int /*n*/, const double *x, double *fvec, double *fjac, - int ldfjac, int iflag) -{ +struct lmder_functor { + static int f(void * /*p*/, int /*m*/, int /*n*/, const double *x, double *fvec, double *fjac, int ldfjac, int iflag) + { - /* subroutine fcn for lmder example. */ + /* subroutine fcn for lmder example. */ - int i; - double tmp1, tmp2, tmp3, tmp4; - double y[15]={1.4e-1, 1.8e-1, 2.2e-1, 2.5e-1, 2.9e-1, 3.2e-1, 3.5e-1, - 3.9e-1, 3.7e-1, 5.8e-1, 7.3e-1, 9.6e-1, 1.34, 2.1, 4.39}; + int i; + double tmp1, tmp2, tmp3, tmp4; + double y[15]={1.4e-1, 1.8e-1, 2.2e-1, 2.5e-1, 2.9e-1, 3.2e-1, 3.5e-1, + 3.9e-1, 3.7e-1, 5.8e-1, 7.3e-1, 9.6e-1, 1.34, 2.1, 4.39}; - if (iflag == 0) - { - /* insert print statements here when nprint is positive. */ - return 0; + if (iflag == 0) + { + /* insert print statements here when nprint is positive. */ + return 0; + } + + if (iflag != 2) + { + for (i=1; i <= 15; i++) + { + tmp1 = i; + tmp2 = 16 - i; + tmp3 = tmp1; + if (i > 8) tmp3 = tmp2; + fvec[i-1] = y[i-1] - (x[1-1] + tmp1/(x[2-1]*tmp2 + x[3-1]*tmp3)); + } + } + else + { + for (i=1; i<=15; i++) + { + tmp1 = i; + tmp2 = 16 - i; + tmp3 = tmp1; + if (i > 8) tmp3 = tmp2; + tmp4 = (x[2-1]*tmp2 + x[3-1]*tmp3); tmp4 = tmp4*tmp4; + fjac[i-1 + ldfjac*(1-1)] = -1.; + fjac[i-1 + ldfjac*(2-1)] = tmp1*tmp2/tmp4; + fjac[i-1 + ldfjac*(3-1)] = tmp1*tmp3/tmp4; + }; + } + return 0; } - - if (iflag != 2) - { - for (i=1; i <= 15; i++) - { - tmp1 = i; - tmp2 = 16 - i; - tmp3 = tmp1; - if (i > 8) tmp3 = tmp2; - fvec[i-1] = y[i-1] - (x[1-1] + tmp1/(x[2-1]*tmp2 + x[3-1]*tmp3)); - } - } - else - { - for (i=1; i<=15; i++) - { - tmp1 = i; - tmp2 = 16 - i; - tmp3 = tmp1; - if (i > 8) tmp3 = tmp2; - tmp4 = (x[2-1]*tmp2 + x[3-1]*tmp3); tmp4 = tmp4*tmp4; - fjac[i-1 + ldfjac*(1-1)] = -1.; - fjac[i-1 + ldfjac*(2-1)] = tmp1*tmp2/tmp4; - fjac[i-1 + ldfjac*(3-1)] = tmp1*tmp3/tmp4; - }; - } - return 0; -} +}; void testLmder() { - int i, j, m, n, ldfjac, maxfev, mode, nprint, info, nfev, njev; - int ipvt[3]; - double ftol, xtol, gtol, factor, fnorm; - double x[3], fvec[15], fjac[15*3], diag[3], qtf[3], - wa1[3], wa2[3], wa3[3], wa4[15]; - double covfac; + const int m=15, n=3; + int info, nfev, njev; + double fnorm, covfac, covar_ftol; + Eigen::VectorXd x(n), fvec(m), diag(n), wa1; + Eigen::MatrixXd fjac(m, n); + VectorXi ipvt; - m = 15; - n = 3; + /* the following starting values provide a rough fit. */ + x.setConstant(n, 1.); -/* the following starting values provide a rough fit. */ - - x[1-1] = 1.; - x[2-1] = 1.; - x[3-1] = 1.; - - ldfjac = 15; - - /* set ftol and xtol to the square root of the machine */ - /* and gtol to zero. unless high solutions are */ - /* required, these are the recommended settings. */ - - ftol = sqrt(dpmpar(1)); - xtol = sqrt(dpmpar(1)); - gtol = 0.; - - maxfev = 400; - mode = 1; - factor = 1.e2; - nprint = 0; - - info = lmder(fcn_lmder, 0, m, n, x, fvec, fjac, ldfjac, ftol, xtol, gtol, - maxfev, diag, mode, factor, nprint, &nfev, &njev, - ipvt, qtf, wa1, wa2, wa3, wa4); - fnorm = enorm(m, fvec); - - VERIFY_IS_APPROX(fnorm, 0.09063596); + // do the computation + info = ei_lmder(x, fvec, nfev, njev, fjac, ipvt, wa1, diag); + // check return values + VERIFY( 1 == info); VERIFY(nfev==6); VERIFY(njev==5); - VERIFY(info==1); - double x_ref[] = {0.08241058, 1.133037, 2.343695 }; - for (j=1; j<=n; j++) VERIFY_IS_APPROX(x[j-1], x_ref[j-1]); - ftol = dpmpar(1); - covfac = fnorm*fnorm/(m-n); - covar(n, fjac, ldfjac, ipvt, ftol, wa1); - double cov_ref[] = { + // check norm + fnorm = fvec.norm(); + VERIFY_IS_APPROX(fnorm, 0.09063596); + + // check x + VectorXd x_ref(n); + x_ref << 0.08241058, 1.133037, 2.343695; + VERIFY_IS_APPROX(x, x_ref); + + // check covariance + covar_ftol = dpmpar(1); + covfac = fnorm*fnorm/(m-n); + covar(n, fjac.data(), m, ipvt.data(), covar_ftol, wa1.data()); + + Eigen::MatrixXd cov_ref(n,n); + cov_ref << 0.0001531202, 0.002869941, -0.002656662, 0.002869941, 0.09480935, -0.09098995, - -0.002656662, -0.09098995, 0.08778727 - }; + -0.002656662, -0.09098995, 0.08778727; - for (i=1; i<=n; i++) - for (j=1; j<=n; j++) - VERIFY_IS_APPROX(fjac[(i-1)*ldfjac+j-1]*covfac, cov_ref[(i-1)*3+(j-1)]); +// std::cout << fjac*covfac << std::endl; + + Eigen::MatrixXd cov; + cov = covfac*fjac.corner(TopLeft); + VERIFY_IS_APPROX( cov, cov_ref); + // TODO: why isn't this allowed ? : + // VERIFY_IS_APPROX( covfac*fjac.corner(TopLeft) , cov_ref); } - int fcn_hybrj1(void * /*p*/, int n, const double *x, double *fvec, double *fjac, int ldfjac, int iflag) {