devtools/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2026-04-10 11:34:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

merging ei_lmdif() and lmdif_template()

Browse Source
This commit is contained in:
Thomas Capricelli
2009-08-21 04:24:59 +02:00
parent 2e3d17c3ce
commit 20480a5438
4 changed files with 63 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,74 +0,0 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_NONLINEAR_MATHFUNCTIONS_H
#define EIGEN_NONLINEAR_MATHFUNCTIONS_H
template<typename Functor, typename Scalar>
int ei_lmdif(
Matrix< Scalar, Dynamic, 1 > &x,
Matrix< Scalar, Dynamic, 1 > &fvec,
int &nfev,
Matrix< Scalar, Dynamic, Dynamic > &fjac,
VectorXi &ipvt,
Matrix< Scalar, Dynamic, 1 > &qtf,
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.),
Scalar epsfcn = Scalar(0.),
int nprint=0
)
{
Matrix< Scalar, Dynamic, 1 >
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());
qtf.resize(x.size());
return lmdif_template<Scalar> (
Functor::f, 0,
fvec.size(), x.size(), x.data(), fvec.data(),
ftol, xtol, gtol,
maxfev,
epsfcn,
diag.data(), mode,
factor,
nprint,
nfev,
fjac.data() , ldfjac,
ipvt.data(),
qtf.data(),
wa1.data(), wa2.data(), wa3.data(), wa4.data()
);
}
#endif // EIGEN_NONLINEAR_MATHFUNCTIONS_H

120
unsupported/Eigen/src/NonLinear/lmdif.h
View File

@@ -1,17 +1,33 @@
template<typename Scalar>
int lmdif_template(minpack_func_mn fcn, void *p, int m, int n, Scalar *x,
Scalar *fvec, Scalar ftol, Scalar xtol, Scalar gtol,
int maxfev, Scalar epsfcn, Scalar *diag, int
mode, Scalar factor, int nprint, int &nfev,
Scalar *fjac, int ldfjac, int *ipvt, Scalar *
qtf, Scalar *wa1, Scalar *wa2, Scalar *wa3, Scalar *
wa4)
template<typename Functor, typename Scalar>
int ei_lmdif(
Matrix< Scalar, Dynamic, 1 > &x,
Matrix< Scalar, Dynamic, 1 > &fvec,
int &nfev,
Matrix< Scalar, Dynamic, Dynamic > &fjac,
VectorXi &ipvt,
Matrix< Scalar, Dynamic, 1 > &qtf,
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.),
Scalar epsfcn = Scalar(0.),
int nprint=0
)
{
/* Initialized data */
const int m = fvec.size(), n = x.size();
Matrix< Scalar, Dynamic, 1 >
wa1(n), wa2(n), wa3(n),
wa4(m);
int ldfjac = m;
/* System generated locals */
int fjac_offset;
ipvt.resize(n);
fjac.resize(ldfjac, n);
diag.resize(n);
qtf.resize(n);
/* Local variables */
int i, j, l;
@@ -25,21 +41,7 @@ int lmdif_template(minpack_func_mn fcn, void *p, int m, int n, Scalar *x,
Scalar pnorm, xnorm, fnorm1, actred, dirder, prered;
int info;
/* Parameter adjustments */
--wa4;
--fvec;
--wa3;
--wa2;
--wa1;
--qtf;
--ipvt;
--diag;
--x;
fjac_offset = 1 + ldfjac;
fjac -= fjac_offset;
/* Function Body */
info = 0;
iflag = 0;
nfev = 0;
@@ -53,7 +55,7 @@ int lmdif_template(minpack_func_mn fcn, void *p, int m, int n, Scalar *x,
if (mode != 2) {
goto L20;
}
for (j = 1; j <= n; ++j) {
for (j = 0; j < n; ++j) {
if (diag[j] <= 0.) {
goto L300;
}
@@ -64,12 +66,12 @@ L20:
/* evaluate the function at the starting point */
/* and calculate its norm. */
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], 1);
iflag = Functor::f(0, m, n, x.data(), fvec.data(), 1);
nfev = 1;
if (iflag < 0) {
goto L300;
}
fnorm = ei_enorm<Scalar>(m, &fvec[1]);
fnorm = fvec.stableNorm();
/* initialize levenberg-marquardt parameter and iteration counter. */
@@ -82,21 +84,21 @@ L30:
/* calculate the jacobian matrix. */
iflag = fdjac2(fcn, p, m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac,
epsfcn, &wa4[1]);
iflag = fdjac2(Functor::f, 0, m, n, x.data(), fvec.data(), fjac.data(), ldfjac,
epsfcn, wa4.data());
nfev += n;
if (iflag < 0) {
goto L300;
}
/* if requested, call fcn to enable printing of iterates. */
/* if requested, call Functor::f to enable printing of iterates. */
if (nprint <= 0) {
goto L40;
}
iflag = 0;
if ((iter - 1) % nprint == 0) {
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], 0);
iflag = Functor::f(0, m, n, x.data(), fvec.data(), 0);
}
if (iflag < 0) {
goto L300;
@@ -105,8 +107,8 @@ L40:
/* compute the qr factorization of the jacobian. */
qrfac(m, n, &fjac[fjac_offset], ldfjac, TRUE_, &ipvt[1], n, &wa1[1], &
wa2[1], &wa3[1]);
qrfac(m, n, fjac.data(), ldfjac, TRUE_, ipvt.data(), n, wa1.data(), wa2.data(), wa3.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. */
@@ -117,7 +119,7 @@ L40:
if (mode == 2) {
goto L60;
}
for (j = 1; j <= n; ++j) {
for (j = 0; j < n; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.) {
diag[j] = 1.;
@@ -129,11 +131,11 @@ L60:
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
for (j = 1; j <= n; ++j) {
for (j = 0; j < n; ++j) {
wa3[j] = diag[j] * x[j];
/* L70: */
}
xnorm = ei_enorm<Scalar>(n, &wa3[1]);
xnorm = wa3.stableNorm();;
delta = factor * xnorm;
if (delta == 0.) {
delta = factor;
@@ -143,21 +145,21 @@ L80:
/* form (q transpose)*fvec and store the first n components in */
/* qtf. */
for (i = 1; i <= m; ++i) {
for (i = 0; i < m; ++i) {
wa4[i] = fvec[i];
/* L90: */
}
for (j = 1; j <= n; ++j) {
for (j = 0; j < n; ++j) {
if (fjac[j + j * ldfjac] == 0.) {
goto L120;
}
sum = 0.;
for (i = j; i <= m; ++i) {
for (i = j; i < m; ++i) {
sum += fjac[i + j * ldfjac] * wa4[i];
/* L100: */
}
temp = -sum / fjac[j + j * ldfjac];
for (i = j; i <= m; ++i) {
for (i = j; i < m; ++i) {
wa4[i] += fjac[i + j * ldfjac] * temp;
/* L110: */
}
@@ -173,13 +175,13 @@ L120:
if (fnorm == 0.) {
goto L170;
}
for (j = 1; j <= n; ++j) {
for (j = 0; j < n; ++j) {
l = ipvt[j];
if (wa2[l] == 0.) {
goto L150;
}
sum = 0.;
for (i = 1; i <= j; ++i) {
for (i = 0; i <= j; ++i) {
sum += fjac[i + j * ldfjac] * (qtf[i] / fnorm);
/* L140: */
}
@@ -205,7 +207,7 @@ L170:
if (mode == 2) {
goto L190;
}
for (j = 1; j <= n; ++j) /* Computing MAX */
for (j = 0; j < n; ++j) /* Computing MAX */
diag[j] = max(diag[j], wa2[j]);
L190:
@@ -215,18 +217,20 @@ L200:
/* determine the levenberg-marquardt parameter. */
lmpar(n, &fjac[fjac_offset], ldfjac, &ipvt[1], &diag[1], &qtf[1], delta,
&par, &wa1[1], &wa2[1], &wa3[1], &wa4[1]);
ipvt.cwise()+=1; // lmpar() expects the fortran convention (as qrfac provides)
lmpar(n, fjac.data(), ldfjac, ipvt.data(), diag.data(), qtf.data(), delta,
&par, wa1.data(), wa2.data(), wa3.data(), wa4.data());
ipvt.cwise()-=1;
/* 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];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
/* L210: */
}
pnorm = ei_enorm<Scalar>(n, &wa3[1]);
pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */
@@ -236,12 +240,12 @@ L200:
/* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, m, n, &wa2[1], &wa4[1], 1);
iflag = Functor::f(0, m, n, wa2.data(), wa4.data(), 1);
++nfev;
if (iflag < 0) {
goto L300;
}
fnorm1 = ei_enorm<Scalar>(m, &wa4[1]);
fnorm1 = wa4.stableNorm();
/* compute the scaled actual reduction. */
@@ -252,18 +256,18 @@ L200:
/* compute the scaled predicted reduction and */
/* the scaled directional derivative. */
for (j = 1; j <= n; ++j) {
for (j = 0; j < n; ++j) {
wa3[j] = 0.;
l = ipvt[j];
temp = wa1[l];
for (i = 1; i <= j; ++i) {
for (i = 0; i <= j; ++i) {
wa3[i] += fjac[i + j * ldfjac] * temp;
/* L220: */
}
/* L230: */
}
temp1 = ei_abs2(ei_enorm<Scalar>(n, &wa3[1]) / fnorm);
temp2 = ei_abs2( ei_sqrt(par) * pnorm / fnorm);
temp1 = ei_abs2(wa3.stableNorm() / fnorm);
temp2 = ei_abs2(ei_sqrt(par) * pnorm / fnorm);
/* Computing 2nd power */
prered = temp1 + temp2 / p5;
dirder = -(temp1 + temp2);
@@ -311,16 +315,16 @@ L260:
/* successful iteration. update x, fvec, and their norms. */
for (j = 1; j <= n; ++j) {
for (j = 0; j < n; ++j) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
/* L270: */
}
for (i = 1; i <= m; ++i) {
for (i = 0; i < m; ++i) {
fvec[i] = wa4[i];
/* L280: */
}
xnorm = ei_enorm<Scalar>(n, &wa2[1]);
xnorm = wa2.stableNorm();
fnorm = fnorm1;
++iter;
L290:
@@ -377,7 +381,7 @@ L300:
}
iflag = 0;
if (nprint > 0) {
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], 0);
iflag = Functor::f(0, m, n, x.data(), fvec.data(), 0);
}
return info;