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

merging ei_hybrd() and hybrd_template()

Browse Source
This commit is contained in:
Thomas Capricelli 2009-08-21 03:13:42 +02:00
parent 1715e2cb3b
commit f2ff0d3903
2 changed files with 79 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,54 +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_hybrd(
Matrix< Scalar, Dynamic, 1 > &x,
Matrix< Scalar, Dynamic, 1 > &fvec,
int &nfev,
Matrix< Scalar, Dynamic, Dynamic > &fjac,
Matrix< Scalar, Dynamic, 1 > &R,
Matrix< Scalar, Dynamic, 1 > &qtf,
Matrix< Scalar, Dynamic, 1 > &diag,
int mode=1,
int nb_of_subdiagonals = -1,
int nb_of_superdiagonals = -1,
int maxfev = 2000,
Scalar factor = Scalar(100.),
Scalar xtol = ei_sqrt(epsilon<Scalar>()),
Scalar epsfcn = Scalar(0.),
int nprint=0
)
{
int n = x.size();
int lr = (n*(n+1))/2;
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;
fvec.resize(n);
qtf.resize(n);
R.resize(lr);
int ldfjac = n;
fjac.resize(ldfjac, n);
return hybrd_template<Scalar>(
Functor::f, 0,
n, x.data(), fvec.data(),
xtol, maxfev,
nb_of_subdiagonals, nb_of_superdiagonals,
epsfcn,
diag.data(), mode,
factor,
nprint,
nfev,
fjac.data(), ldfjac,
R.data(), lr,
qtf.data(),
wa1.data(), wa2.data(), wa3.data(), wa4.data()
);
}
template<typename Functor, typename Scalar> template<typename Functor, typename Scalar>
int ei_hybrj( int ei_hybrj(

154
unsupported/Eigen/src/NonLinear/hybrd.h
View File

@ -1,18 +1,38 @@
template<typename Scalar> template<typename Functor, typename Scalar>
int hybrd_template(minpack_func_nn fcn, void *p, int n, Scalar *x, Scalar * int ei_hybrd(
fvec, Scalar xtol, int maxfev, int ml, int mu, Matrix< Scalar, Dynamic, 1 > &x,
Scalar epsfcn, Scalar *diag, int mode, Scalar factor, int nprint, int &nfev, Scalar * Matrix< Scalar, Dynamic, 1 > &fvec,
fjac, int ldfjac, Scalar *r__, int lr, Scalar *qtf, int &nfev,
Scalar *wa1, Scalar *wa2, Scalar *wa3, Scalar *wa4) Matrix< Scalar, Dynamic, Dynamic > &fjac,
Matrix< Scalar, Dynamic, 1 > &R,
Matrix< Scalar, Dynamic, 1 > &qtf,
Matrix< Scalar, Dynamic, 1 > &diag,
int mode=1,
int nb_of_subdiagonals = -1,
int nb_of_superdiagonals = -1,
int maxfev = 2000,
Scalar factor = Scalar(100.),
Scalar xtol = ei_sqrt(epsilon<Scalar>()),
Scalar epsfcn = Scalar(0.),
int nprint=0
)
{ {
/* Initialized data */ const int n = x.size();
int lr = (n*(n+1))/2;
Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n), wa3(n), wa4(n);
/* System generated locals */
int fjac_offset; if (nb_of_subdiagonals<0) nb_of_subdiagonals = n-1;
if (nb_of_superdiagonals<0) nb_of_superdiagonals = n-1;
fvec.resize(n);
qtf.resize(n);
R.resize(lr);
int ldfjac = n;
fjac.resize(ldfjac, n);
/* Local variables */ /* Local variables */
int i, j, l, jm1, iwa[1]; int i, j, l, iwa[1];
Scalar sum; Scalar sum;
int sing; int sing;
int iter; int iter;
@ -29,19 +49,6 @@ int hybrd_template(minpack_func_nn fcn, void *p, int n, Scalar *x, Scalar *
Scalar actred, prered; Scalar actred, prered;
int info; int info;
/* Parameter adjustments */
--wa4;
--wa3;
--wa2;
--wa1;
--qtf;
--diag;
--fvec;
--x;
fjac_offset = 1 + ldfjac;
fjac -= fjac_offset;
--r__;
/* Function Body */ /* Function Body */
info = 0; info = 0;
@ -50,14 +57,14 @@ int hybrd_template(minpack_func_nn fcn, void *p, int n, Scalar *x, Scalar *
/* check the input parameters for errors. */ /* check the input parameters for errors. */
if (n <= 0 || xtol < 0. || maxfev <= 0 || ml < 0 || mu < 0 || if (n <= 0 || xtol < 0. || maxfev <= 0 || nb_of_subdiagonals < 0 || nb_of_superdiagonals < 0 ||
factor <= 0. || ldfjac < n || lr < n * (n + 1) / 2) { factor <= 0. || ldfjac < n || lr < n * (n + 1) / 2) {
goto L300; goto L300;
} }
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 L300; goto L300;
} }
@ -68,18 +75,18 @@ 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, n, &x[1], &fvec[1], 1); iflag = Functor::f(0, n, x.data(), fvec.data(), 1);
nfev = 1; nfev = 1;
if (iflag < 0) { if (iflag < 0) {
goto L300; goto L300;
} }
fnorm = ei_enorm<Scalar>(n, &fvec[1]); fnorm = fvec.stableNorm();
/* determine the number of calls to fcn needed to compute */ /* determine the number of calls to fcn needed to compute */
/* the jacobian matrix. */ /* the jacobian matrix. */
/* Computing MIN */ /* Computing MIN */
msum = min(ml + mu + 1, n); msum = min(nb_of_subdiagonals + nb_of_superdiagonals + 1, n);
/* initialize iteration counter and monitors. */ /* initialize iteration counter and monitors. */
@ -96,8 +103,8 @@ L30:
/* calculate the jacobian matrix. */ /* calculate the jacobian matrix. */
iflag = fdjac1(fcn, p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, iflag = fdjac1(Functor::f, 0, n, x.data(), fvec.data(), fjac.data(), ldfjac,
ml, mu, epsfcn, &wa1[1], &wa2[1]); nb_of_subdiagonals, nb_of_superdiagonals, epsfcn, wa1.data(), wa2.data());
nfev += msum; nfev += msum;
if (iflag < 0) { if (iflag < 0) {
goto L300; goto L300;
@ -105,8 +112,7 @@ L30:
/* compute the qr factorization of the jacobian. */ /* compute the qr factorization of the jacobian. */
qrfac(n, n, &fjac[fjac_offset], ldfjac, FALSE_, iwa, 1, &wa1[1], & qrfac(n, n, fjac.data(), ldfjac, FALSE_, iwa, 1, wa1.data(), wa2.data(), wa3.data());
wa2[1], &wa3[1]);
/* on the first iteration and if mode is 1, scale according */ /* on the first iteration and if mode is 1, scale according */
/* to the norms of the columns of the initial jacobian. */ /* to the norms of the columns of the initial jacobian. */
@ -117,7 +123,7 @@ L30:
if (mode == 2) { if (mode == 2) {
goto L50; goto L50;
} }
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.;
@ -129,11 +135,11 @@ L50:
/* 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];
/* L60: */ /* L60: */
} }
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;
@ -142,22 +148,22 @@ L70:
/* form (q transpose)*fvec and store in qtf. */ /* form (q transpose)*fvec and store in qtf. */
for (i = 1; i <= n; ++i) { for (i = 0; i < n; ++i) {
qtf[i] = fvec[i]; qtf[i] = fvec[i];
/* L80: */ /* L80: */
} }
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
if (fjac[j + j * ldfjac] == 0.) { if (fjac(j,j) == 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:
@ -168,19 +174,16 @@ L110:
/* copy the triangular factor of the qr factorization into r. */ /* copy the triangular factor of the qr factorization into r. */
sing = FALSE_; sing = FALSE_;
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
l = j; l = j;
jm1 = j - 1; if (j) {
if (jm1 < 1) { for (i = 0; i < j; ++i) {
goto L140; R[l] = fjac(i,j);
} l = l + n - i -1;
for (i = 1; i <= jm1; ++i) {
r__[l] = fjac[i + j * ldfjac];
l = l + n - i;
/* L130: */ /* L130: */
} }
L140: }
r__[l] = wa1[j]; R[l] = wa1[j];
if (wa1[j] == 0.) { if (wa1[j] == 0.) {
sing = TRUE_; sing = TRUE_;
} }
@ -189,7 +192,7 @@ L140:
/* accumulate the orthogonal factor in fjac. */ /* accumulate the orthogonal factor in fjac. */
qform(n, n, &fjac[fjac_offset], ldfjac, &wa1[1]); qform(n, n, fjac.data(), ldfjac, wa1.data());
/* rescale if necessary. */ /* rescale if necessary. */
@ -197,7 +200,7 @@ L140:
goto L170; goto L170;
} }
/* Computing MAX */ /* Computing MAX */
for (j = 1; j <= n; ++j) for (j = 0; j < n; ++j)
diag[j] = max(diag[j], wa2[j]); diag[j] = max(diag[j], wa2[j]);
L170: L170:
@ -212,7 +215,7 @@ L180:
} }
iflag = 0; iflag = 0;
if ((iter - 1) % nprint == 0) { if ((iter - 1) % nprint == 0) {
iflag = (*fcn)(p, n, &x[1], &fvec[1], 0); iflag = Functor::f(0, n, x.data(), fvec.data(), 0);
} }
if (iflag < 0) { if (iflag < 0) {
goto L300; goto L300;
@ -221,18 +224,17 @@ L190:
/* determine the direction p. */ /* determine the direction p. */
dogleg(n, &r__[1], lr, &diag[1], &qtf[1], delta, &wa1[1], &wa2[1], &wa3[ dogleg(n, R.data(), lr, diag.data(), qtf.data(), delta, wa1.data(), wa2.data(), wa3.data());
1]);
/* 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];
/* L200: */ /* L200: */
} }
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. */
@ -242,12 +244,12 @@ L190:
/* evaluate the function at x + p and calculate its norm. */ /* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, n, &wa2[1], &wa4[1], 1); iflag = Functor::f(0, n, wa2.data(), wa4.data(), 1);
++(nfev); ++(nfev);
if (iflag < 0) { if (iflag < 0) {
goto L300; goto L300;
} }
fnorm1 = ei_enorm<Scalar>(n, &wa4[1]); fnorm1 = wa4.stableNorm();
/* compute the scaled actual reduction. */ /* compute the scaled actual reduction. */
@ -257,18 +259,18 @@ L190:
/* compute the scaled predicted reduction. */ /* compute the scaled predicted reduction. */
l = 1; l = 0;
for (i = 1; i <= n; ++i) { for (i = 0; i < n; ++i) {
sum = 0.; sum = 0.;
for (j = i; j <= n; ++j) { for (j = i; j < n; ++j) {
sum += r__[l] * wa1[j]; sum += R[l] * wa1[j];
++l; ++l;
/* L210: */ /* L210: */
} }
wa3[i] = qtf[i] + sum; wa3[i] = qtf[i] + sum;
/* L220: */ /* L220: */
} }
temp = ei_enorm<Scalar>(n, &wa3[1]); temp = wa3.stableNorm();
prered = 0.; prered = 0.;
if (temp < fnorm) /* Computing 2nd power */ if (temp < fnorm) /* Computing 2nd power */
prered = 1. - ei_abs2(temp / fnorm); prered = 1. - ei_abs2(temp / fnorm);
@ -308,13 +310,13 @@ L240:
/* 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];
fvec[j] = wa4[j]; fvec[j] = wa4[j];
/* L250: */ /* L250: */
} }
xnorm = ei_enorm<Scalar>(n, &wa2[1]); temp = wa2.stableNorm();
fnorm = fnorm1; fnorm = fnorm1;
++iter; ++iter;
L260: L260:
@ -365,10 +367,10 @@ L260:
/* calculate the rank one modification to the jacobian */ /* calculate the rank one modification to the jacobian */
/* and update qtf if necessary. */ /* and update qtf if necessary. */
for (j = 1; j <= n; ++j) { for (j = 0; j < n; ++j) {
sum = 0.; sum = 0.;
for (i = 1; i <= n; ++i) { for (i = 0; i < n; ++i) {
sum += fjac[i + j * ldfjac] * wa4[i]; sum += fjac(i,j) * wa4[i];
/* L270: */ /* L270: */
} }
wa2[j] = (sum - wa3[j]) / pnorm; wa2[j] = (sum - wa3[j]) / pnorm;
@ -381,9 +383,9 @@ L260:
/* compute the qr factorization of the updated jacobian. */ /* compute the qr factorization of the updated jacobian. */
r1updt(n, n, &r__[1], lr, &wa1[1], &wa2[1], &wa3[1], &sing); r1updt(n, n, R.data(), lr, wa1.data(), wa2.data(), wa3.data(), &sing);
r1mpyq(n, n, &fjac[fjac_offset], ldfjac, &wa2[1], &wa3[1]); r1mpyq(n, n, fjac.data(), ldfjac, wa2.data(), wa3.data());
r1mpyq(1, n, &qtf[1], 1, &wa2[1], &wa3[1]); r1mpyq(1, n, qtf.data(), 1, wa2.data(), wa3.data());
/* end of the inner loop. */ /* end of the inner loop. */
@ -402,7 +404,7 @@ L300:
info = iflag; info = iflag;
} }
if (nprint > 0) { if (nprint > 0) {
(*fcn)(p, n, &x[1], &fvec[1], 0); Functor::f(0, n, x.data(), fvec.data(), 0);
} }
return info; return info;