GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-06-20 22:52:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

merging ei_lmstr() and lmstr_template()

Browse Source
This commit is contained in:
Thomas Capricelli 2009-08-21 03:41:19 +02:00
parent e48b6ad905
commit 524e112ee5
2 changed files with 75 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
unsupported/Eigen/src/NonLinear/MathFunctions.h
View File

@ -25,49 +25,6 @@
#ifndef EIGEN_NONLINEAR_MATHFUNCTIONS_H #ifndef EIGEN_NONLINEAR_MATHFUNCTIONS_H
#define EIGEN_NONLINEAR_MATHFUNCTIONS_H #define EIGEN_NONLINEAR_MATHFUNCTIONS_H
template<typename Functor, typename Scalar>
int ei_lmstr(
Matrix< Scalar, Dynamic, 1 > &x,
Matrix< Scalar, Dynamic, 1 > &fvec,
int &nfev,
int &njev,
Matrix< Scalar, Dynamic, Dynamic > &fjac,
VectorXi &ipvt,
Matrix< Scalar, Dynamic, 1 > &diag,
int mode=1,
Scalar factor = 100.,
int maxfev = 400,
Scalar ftol = ei_sqrt(epsilon<Scalar>()),
Scalar xtol = ei_sqrt(epsilon<Scalar>()),
Scalar gtol = Scalar(0.),
int nprint=0
)
{
Matrix< Scalar, Dynamic, 1 >
qtf(x.size()),
wa1(x.size()), wa2(x.size()), wa3(x.size()),
wa4(fvec.size());
int ldfjac = fvec.size();
ipvt.resize(x.size());
fjac.resize(ldfjac, x.size());
diag.resize(x.size());
return lmstr_template<Scalar> (
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()
);
}
template<typename Functor, typename Scalar> template<typename Functor, typename Scalar>
int ei_lmdif( int ei_lmdif(
Matrix< Scalar, Dynamic, 1 > &x, Matrix< Scalar, Dynamic, 1 > &x,

148
unsupported/Eigen/src/NonLinear/lmstr.h
View File

@ -1,16 +1,32 @@
template<typename Scalar> template<typename Functor, typename Scalar>
int lmstr_template(minpack_funcderstr_mn fcn, void *p, int m, int n, Scalar *x, int ei_lmstr(
Scalar *fvec, Scalar *fjac, int ldfjac, Scalar ftol, Matrix< Scalar, Dynamic, 1 > &x,
Scalar xtol, Scalar gtol, int maxfev, Scalar * Matrix< Scalar, Dynamic, 1 > &fvec,
diag, int mode, Scalar factor, int nprint, int &nfev,
int &nfev, int &njev, int *ipvt, Scalar *qtf, int &njev,
Scalar *wa1, Scalar *wa2, Scalar *wa3, Scalar *wa4) Matrix< Scalar, Dynamic, Dynamic > &fjac,
VectorXi &ipvt,
Matrix< Scalar, Dynamic, 1 > &diag,
int mode=1,
Scalar factor = 100.,
int maxfev = 400,
Scalar ftol = ei_sqrt(epsilon<Scalar>()),
Scalar xtol = ei_sqrt(epsilon<Scalar>()),
Scalar gtol = Scalar(0.),
int nprint=0
)
{ {
/* Initialized data */ const int m = fvec.size(), n = x.size();
Matrix< Scalar, Dynamic, 1 >
qtf(n),
wa1(n), wa2(n), wa3(n),
wa4(m);
int ldfjac = m;
/* System generated locals */ ipvt.resize(n);
int fjac_offset; fjac.resize(ldfjac, n);
diag.resize(n);
/* Local variables */ /* Local variables */
int i, j, l; int i, j, l;
@ -24,21 +40,7 @@ int lmstr_template(minpack_funcderstr_mn fcn, void *p, int m, int n, Scalar *x,
Scalar fnorm, gnorm, pnorm, xnorm, fnorm1, actred, dirder, prered; Scalar fnorm, gnorm, pnorm, xnorm, fnorm1, actred, dirder, prered;
int info; int info;
/* Parameter adjustments */
--wa4;
--fvec;
--wa3;
--wa2;
--wa1;
--qtf;
--ipvt;
--diag;
--x;
fjac_offset = 1 + ldfjac;
fjac -= fjac_offset;
/* Function Body */ /* Function Body */
info = 0; info = 0;
iflag = 0; iflag = 0;
nfev = 0; nfev = 0;
@ -53,7 +55,7 @@ int lmstr_template(minpack_funcderstr_mn fcn, void *p, int m, int n, Scalar *x,
if (mode != 2) { if (mode != 2) {
goto L20; goto L20;
} }
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
if (diag[j] <= 0.) { if (diag[j] <= 0.) {
goto L340; goto L340;
} }
@ -64,12 +66,12 @@ L20:
/* evaluate the function at the starting point */ /* evaluate the function at the starting point */
/* and calculate its norm. */ /* and calculate its norm. */
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], &wa3[1], 1); iflag = Functor::f(0, m, n, x.data(), fvec.data(), wa3.data(), 1);
nfev = 1; nfev = 1;
if (iflag < 0) { if (iflag < 0) {
goto L340; goto L340;
} }
fnorm = ei_enorm<Scalar>(m, &fvec[1]); fnorm = fvec.stableNorm();;
/* initialize levenberg-marquardt parameter and iteration counter. */ /* initialize levenberg-marquardt parameter and iteration counter. */
@ -80,14 +82,14 @@ L20:
L30: L30:
/* if requested, call fcn to enable printing of iterates. */ /* if requested, call Functor::f to enable printing of iterates. */
if (nprint <= 0) { if (nprint <= 0) {
goto L40; goto L40;
} }
iflag = 0; iflag = 0;
if ((iter - 1) % nprint == 0) { if ((iter - 1) % nprint == 0) {
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], &wa3[1], 0); iflag = Functor::f(0, m, n, x.data(), fvec.data(), wa3.data(), 0);
} }
if (iflag < 0) { if (iflag < 0) {
goto L340; goto L340;
@ -99,22 +101,21 @@ L40:
/* forming (q transpose)*fvec and storing the first */ /* forming (q transpose)*fvec and storing the first */
/* n components in qtf. */ /* n components in qtf. */
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
qtf[j] = 0.; qtf[j] = 0.;
for (i = 1; i <= n; ++i) { for (i = 0; i < n; ++i) {
fjac[i + j * ldfjac] = 0.; fjac(i,j) = 0.;
/* L50: */ /* L50: */
} }
/* L60: */ /* L60: */
} }
iflag = 2; iflag = 2;
for (i = 1; i <= m; ++i) { for (i = 0; i < m; ++i) {
if ((*fcn)(p, m, n, &x[1], &fvec[1], &wa3[1], iflag) < 0) { if (Functor::f(0, m, n, x.data(), fvec.data(), wa3.data(), iflag) < 0) {
goto L340; goto L340;
} }
temp = fvec[i]; temp = fvec[i];
rwupdt(n, &fjac[fjac_offset], ldfjac, &wa3[1], &qtf[1], &temp, &wa1[ rwupdt(n, fjac.data(), ldfjac, wa3.data(), qtf.data(), &temp, wa1.data(), wa2.data());
1], &wa2[1]);
++iflag; ++iflag;
/* L70: */ /* L70: */
} }
@ -124,35 +125,36 @@ L40:
/* reorder its columns and update the components of qtf. */ /* reorder its columns and update the components of qtf. */
sing = FALSE_; sing = FALSE_;
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
if (fjac[j + j * ldfjac] == 0.) { if (fjac(j,j) == 0.) {
sing = TRUE_; sing = TRUE_;
} }
ipvt[j] = j; ipvt[j] = j+1;
wa2[j] = ei_enorm<Scalar>(j, &fjac[j * ldfjac + 1]); wa2[j] = fjac.col(j).start(j).stableNorm();
// wa2[j] = ei_enorm<Scalar>(j, &fjac[j * ldfjac + 1]);
// sum += fjac[i + j * ldfjac] * (qtf[i] / fnorm);
/* L80: */ /* L80: */
} }
if (! sing) { if (! sing) {
goto L130; goto L130;
} }
qrfac(n, n, &fjac[fjac_offset], ldfjac, TRUE_, &ipvt[1], n, &wa1[1], & qrfac(n, n, fjac.data(), ldfjac, TRUE_, ipvt.data(), n, wa1.data(), wa2.data(), wa3.data());
wa2[1], &wa3[1]); for (j = 0; j < n; ++j) {
for (j = 1; j <= n; ++j) { if (fjac(j,j) == 0.) {
if (fjac[j + j * ldfjac] == 0.) {
goto L110; goto L110;
} }
sum = 0.; sum = 0.;
for (i = j; i <= n; ++i) { for (i = j; i < n; ++i) {
sum += fjac[i + j * ldfjac] * qtf[i]; sum += fjac(i,j) * qtf[i];
/* L90: */ /* L90: */
} }
temp = -sum / fjac[j + j * ldfjac]; temp = -sum / fjac(j,j);
for (i = j; i <= n; ++i) { for (i = j; i < n; ++i) {
qtf[i] += fjac[i + j * ldfjac] * temp; qtf[i] += fjac(i,j) * temp;
/* L100: */ /* L100: */
} }
L110: L110:
fjac[j + j * ldfjac] = wa1[j]; fjac(j,j) = wa1[j];
/* L120: */ /* L120: */
} }
L130: L130:
@ -166,7 +168,7 @@ L130:
if (mode == 2) { if (mode == 2) {
goto L150; goto L150;
} }
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
diag[j] = wa2[j]; diag[j] = wa2[j];
if (wa2[j] == 0.) { if (wa2[j] == 0.) {
diag[j] = 1.; diag[j] = 1.;
@ -178,11 +180,11 @@ L150:
/* on the first iteration, calculate the norm of the scaled x */ /* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */ /* and initialize the step bound delta. */
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
wa3[j] = diag[j] * x[j]; wa3[j] = diag[j] * x[j];
/* L160: */ /* L160: */
} }
xnorm = ei_enorm<Scalar>(n, &wa3[1]); xnorm = wa3.stableNorm();
delta = factor * xnorm; delta = factor * xnorm;
if (delta == 0.) { if (delta == 0.) {
delta = factor; delta = factor;
@ -195,14 +197,14 @@ L170:
if (fnorm == 0.) { if (fnorm == 0.) {
goto L210; goto L210;
} }
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
l = ipvt[j]; l = ipvt[j]-1;
if (wa2[l] == 0.) { if (wa2[l] == 0.) {
goto L190; goto L190;
} }
sum = 0.; sum = 0.;
for (i = 1; i <= j; ++i) { for (i = 0; i < j; ++i) {
sum += fjac[i + j * ldfjac] * (qtf[i] / fnorm); sum += fjac(i,j) * (qtf[i] / fnorm);
/* L180: */ /* L180: */
} }
/* Computing MAX */ /* Computing MAX */
@ -227,7 +229,7 @@ L210:
if (mode == 2) { if (mode == 2) {
goto L230; goto L230;
} }
for (j = 1; j <= n; ++j) /* Computing MAX */ for (j = 0; j < n; ++j) /* Computing MAX */
diag[j] = max(diag[j], wa2[j]); diag[j] = max(diag[j], wa2[j]);
L230: L230:
@ -237,18 +239,18 @@ L240:
/* determine the levenberg-marquardt parameter. */ /* determine the levenberg-marquardt parameter. */
lmpar(n, &fjac[fjac_offset], ldfjac, &ipvt[1], &diag[1], &qtf[1], delta, lmpar(n, fjac.data(), ldfjac, ipvt.data(), diag.data(), qtf.data(), delta, &par,
&par, &wa1[1], &wa2[1], &wa3[1], &wa4[1]); wa1.data(), wa2.data(), wa3.data(), wa4.data());
/* store the direction p and x + p. calculate the norm of p. */ /* store the direction p and x + p. calculate the norm of p. */
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
wa1[j] = -wa1[j]; wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j]; wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j]; wa3[j] = diag[j] * wa1[j];
/* L250: */ /* L250: */
} }
pnorm = ei_enorm<Scalar>(n, &wa3[1]); pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */ /* on the first iteration, adjust the initial step bound. */
@ -258,12 +260,12 @@ L240:
/* evaluate the function at x + p and calculate its norm. */ /* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, m, n, &wa2[1], &wa4[1], &wa3[1], 1); iflag = Functor::f(0, m, n, wa2.data(), wa4.data(), wa3.data(), 1);
++nfev; ++nfev;
if (iflag < 0) { if (iflag < 0) {
goto L340; goto L340;
} }
fnorm1 = ei_enorm<Scalar>(m, &wa4[1]); fnorm1 = wa4.stableNorm();
/* compute the scaled actual reduction. */ /* compute the scaled actual reduction. */
@ -274,17 +276,17 @@ L240:
/* compute the scaled predicted reduction and */ /* compute the scaled predicted reduction and */
/* the scaled directional derivative. */ /* the scaled directional derivative. */
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
wa3[j] = 0.; wa3[j] = 0.;
l = ipvt[j]; l = ipvt[j]-1;
temp = wa1[l]; temp = wa1[l];
for (i = 1; i <= j; ++i) { for (i = 0; i <= j; ++i) {
wa3[i] += fjac[i + j * ldfjac] * temp; wa3[i] += fjac(i,j) * temp;
/* L260: */ /* L260: */
} }
/* L270: */ /* L270: */
} }
temp1 = ei_abs2(ei_enorm<Scalar>(n, &wa3[1]) / fnorm); temp1 = ei_abs2(wa3.stableNorm() / fnorm);
temp2 = ei_abs2( ei_sqrt(par) * pnorm / fnorm); temp2 = ei_abs2( ei_sqrt(par) * pnorm / fnorm);
/* Computing 2nd power */ /* Computing 2nd power */
prered = temp1 + temp2 / p5; prered = temp1 + temp2 / p5;
@ -334,16 +336,16 @@ L300:
/* successful iteration. update x, fvec, and their norms. */ /* successful iteration. update x, fvec, and their norms. */
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
x[j] = wa2[j]; x[j] = wa2[j];
wa2[j] = diag[j] * x[j]; wa2[j] = diag[j] * x[j];
/* L310: */ /* L310: */
} }
for (i = 1; i <= m; ++i) { for (i = 0; i < m; ++i) {
fvec[i] = wa4[i]; fvec[i] = wa4[i];
/* L320: */ /* L320: */
} }
xnorm = ei_enorm<Scalar>(n, &wa2[1]); xnorm = wa2.stableNorm();
fnorm = fnorm1; fnorm = fnorm1;
++iter; ++iter;
L330: L330:
@ -400,7 +402,7 @@ L340:
} }
iflag = 0; iflag = 0;
if (nprint > 0) { if (nprint > 0) {
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], &wa3[1], 0); iflag = Functor::f(0, m, n, x.data(), fvec.data(), wa3.data(), 0);
} }
return info; return info;