mirror/eigen
2
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-03-01 18:26:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

import main files from cminpack as *.h files:

Browse Source 
* function names are changed by appending _template
* it uses basic templating : template<typename T>
* wrappers now use those versions instead of the ones from cminpack
* lot of external methods from cminpack are still used
* tests pass though they are unchanged (they use wrappers)
This commit is contained in:
Thomas Capricelli 2009-08-19 18:38:45 +02:00
parent 703198a1a6
commit fb54cfb013
14 changed files with 2670 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
unsupported/Eigen/NonLinear
View File

@ -1,3 +1,28 @@
// 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_MODULE_H
#define EIGEN_NONLINEAR_MODULE_H
@ -11,6 +36,30 @@ namespace Eigen {
*/
//@{
#define min(a,b) ((a) <= (b) ? (a) : (b))
#define max(a,b) ((a) >= (b) ? (a) : (b))
#define abs(x) ((x) >= 0 ? (x) : -(x))
#define TRUE_ (1)
#define FALSE_ (0)
#define p1 .1
#define p5 .5
#define p25 .25
#define p75 .75
#define p001 .001
#define p0001 1e-4
#include <cminpack.h>
#include "src/NonLinear/lmder1.h"
#include "src/NonLinear/lmder.h"
#include "src/NonLinear/hybrd1.h"
#include "src/NonLinear/hybrd.h"
#include "src/NonLinear/lmstr1.h"
#include "src/NonLinear/lmstr.h"
#include "src/NonLinear/lmdif1.h"
#include "src/NonLinear/lmdif.h"
#include "src/NonLinear/hybrj1.h"
#include "src/NonLinear/hybrj.h"
#include "src/NonLinear/chkder.h"
#include "src/NonLinear/MathFunctions.h"
//@}

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

@ -25,8 +25,6 @@
#ifndef EIGEN_NONLINEAR_MATHFUNCTIONS_H
#define EIGEN_NONLINEAR_MATHFUNCTIONS_H
#include <cminpack.h>
template<typename Functor, typename Scalar>
int ei_hybrd1(
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &x,
@ -38,7 +36,7 @@ int ei_hybrd1(
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);
return hybrd1_template<double>(Functor::f, 0, x.size(), x.data(), fvec.data(), tol, wa.data(), lwa);
}
template<typename Functor, typename Scalar>
@ -72,7 +70,7 @@ int ei_hybrd(
R.resize(lr);
int ldfjac = n;
fjac.resize(ldfjac, n);
return hybrd(
return hybrd_template<double>(
Functor::f, 0,
n, x.data(), fvec.data(),
xtol, maxfev,
@ -105,7 +103,7 @@ int ei_hybrj1(
fvec.resize(n);
fjac.resize(ldfjac, n);
return hybrj1(Functor::f, 0, n, x.data(), fvec.data(), fjac.data(), ldfjac, tol, wa.data(), lwa);
return hybrj1_template<double>(Functor::f, 0, n, x.data(), fvec.data(), fjac.data(), ldfjac, tol, wa.data(), lwa);
}
@ -135,7 +133,7 @@ int ei_hybrj(
R.resize(lr);
int ldfjac = n;
fjac.resize(ldfjac, n);
return hybrj (
return hybrj_template<double> (
Functor::f, 0,
n, x.data(), fvec.data(),
fjac.data(), ldfjac,
@ -165,7 +163,7 @@ int ei_lmstr1(
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > fjac(ldfjac, x.size());
ipvt.resize(x.size());
return lmstr1 (
return lmstr1_template<double>(
Functor::f, 0,
fvec.size(), x.size(), x.data(), fvec.data(),
fjac.data() , ldfjac,
@ -202,7 +200,7 @@ int ei_lmstr(
ipvt.resize(x.size());
fjac.resize(ldfjac, x.size());
diag.resize(x.size());
return lmstr (
return lmstr_template<double> (
Functor::f, 0,
fvec.size(), x.size(), x.data(), fvec.data(),
fjac.data() , ldfjac,
@ -234,7 +232,7 @@ int ei_lmder1(
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > fjac(ldfjac, x.size());
ipvt.resize(x.size());
return lmder1 (
return lmder1_template <double> (
Functor::f, 0,
fvec.size(), x.size(), x.data(), fvec.data(),
fjac.data() , ldfjac,
@ -271,7 +269,7 @@ int ei_lmder(
ipvt.resize(x.size());
fjac.resize(ldfjac, x.size());
diag.resize(x.size());
return lmder (
return lmder_template<double>(
Functor::f, 0,
fvec.size(), x.size(), x.data(), fvec.data(),
fjac.data() , ldfjac,
@ -314,7 +312,7 @@ int ei_lmdif(
ipvt.resize(x.size());
fjac.resize(ldfjac, x.size());
diag.resize(x.size());
return lmdif (
return lmdif_template<double> (
Functor::f, 0,
fvec.size(), x.size(), x.data(), fvec.data(),
ftol, xtol, gtol,
@ -347,7 +345,7 @@ int ei_lmdif1(
iwa.resize(n);
wa.resize(lwa);
return lmdif1 (
return lmdif1_template<double> (
Functor::f, 0,
fvec.size(), n, x.data(), fvec.data(),
tol,
@ -372,7 +370,7 @@ void ei_chkder(
xp.resize(ldfjac);
else
err.resize(ldfjac);
chkder(
chkder_template<double>(
fvec.size(), x.size(), x.data(), fvec.data(),
fjac.data(), ldfjac,
xp.data(),

105
unsupported/Eigen/src/NonLinear/chkder.h Normal file
View File

@ -0,0 +1,105 @@
#define chkder_log10e 0.43429448190325182765
#define chkder_factor 100.
/* Table of constant values */
template<typename T>
void chkder_template(int m, int n, const T *x,
T *fvec, T *fjac, int ldfjac, T *xp,
T *fvecp, int mode, T *err)
{
/* System generated locals */
int fjac_dim1, fjac_offset, i__1, i__2;
/* Local variables */
int i__, j;
T eps, epsf, temp, epsmch;
T epslog;
/* Parameter adjustments */
--err;
--fvecp;
--fvec;
--xp;
--x;
fjac_dim1 = ldfjac;
fjac_offset = 1 + fjac_dim1 * 1;
fjac -= fjac_offset;
/* Function Body */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
eps = sqrt(epsmch);
if (mode == 2) {
goto L20;
}
/* mode = 1. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
temp = eps * fabs(x[j]);
if (temp == 0.) {
temp = eps;
}
xp[j] = x[j] + temp;
/* L10: */
}
/* goto L70; */
return;
L20:
/* mode = 2. */
epsf = chkder_factor * epsmch;
epslog = chkder_log10e * log(eps);
i__1 = m;
for (i__ = 1; i__ <= i__1; ++i__) {
err[i__] = 0.;
/* L30: */
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
temp = fabs(x[j]);
if (temp == 0.) {
temp = 1.;
}
i__2 = m;
for (i__ = 1; i__ <= i__2; ++i__) {
err[i__] += temp * fjac[i__ + j * fjac_dim1];
/* L40: */
}
/* L50: */
}
i__1 = m;
for (i__ = 1; i__ <= i__1; ++i__) {
temp = 1.;
if (fvec[i__] != 0. && fvecp[i__] != 0. && fabs(fvecp[i__] -
fvec[i__]) >= epsf * fabs(fvec[i__]))
{
temp = eps * fabs((fvecp[i__] - fvec[i__]) / eps - err[i__])
/ (fabs(fvec[i__]) +
fabs(fvecp[i__]));
}
err[i__] = 1.;
if (temp > epsmch && temp < eps) {
err[i__] = (chkder_log10e * log(temp) - epslog) / epslog;
}
if (temp >= eps) {
err[i__] = 0.;
}
/* L60: */
}
/* L70: */
/* return 0; */
/* last card of subroutine chkder. */
} /* chkder_ */

454
unsupported/Eigen/src/NonLinear/hybrd.h Normal file
View File

@ -0,0 +1,454 @@
template<typename T>
int hybrd_template(minpack_func_nn fcn, void *p, int n, T *x, T *
fvec, T xtol, int maxfev, int ml, int mu,
T epsfcn, T *diag, int mode, T
factor, int nprint, int *nfev, T *
fjac, int ldfjac, T *r__, int lr, T *qtf,
T *wa1, T *wa2, T *wa3, T *wa4)
{
/* Initialized data */
/* System generated locals */
int fjac_dim1, fjac_offset, i__1, i__2;
T d__1, d__2;
/* Local variables */
int i__, j, l, jm1, iwa[1];
T sum;
int sing;
int iter;
T temp;
int msum, iflag;
T delta;
int jeval;
int ncsuc;
T ratio;
T fnorm;
T pnorm, xnorm, fnorm1;
int nslow1, nslow2;
int ncfail;
T actred, epsmch, prered;
int info;
/* Parameter adjustments */
--wa4;
--wa3;
--wa2;
--wa1;
--qtf;
--diag;
--fvec;
--x;
fjac_dim1 = ldfjac;
fjac_offset = 1 + fjac_dim1 * 1;
fjac -= fjac_offset;
--r__;
/* Function Body */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
info = 0;
iflag = 0;
*nfev = 0;
/* check the input parameters for errors. */
if (n <= 0 || xtol < 0. || maxfev <= 0 || ml < 0 || mu < 0 ||
factor <= 0. || ldfjac < n || lr < n * (n + 1) / 2) {
goto L300;
}
if (mode != 2) {
goto L20;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
if (diag[j] <= 0.) {
goto L300;
}
/* L10: */
}
L20:
/* evaluate the function at the starting point */
/* and calculate its norm. */
iflag = (*fcn)(p, n, &x[1], &fvec[1], 1);
*nfev = 1;
if (iflag < 0) {
goto L300;
}
fnorm = enorm(n, &fvec[1]);
/* determine the number of calls to fcn needed to compute */
/* the jacobian matrix. */
/* Computing MIN */
i__1 = ml + mu + 1;
msum = min(i__1,n);
/* initialize iteration counter and monitors. */
iter = 1;
ncsuc = 0;
ncfail = 0;
nslow1 = 0;
nslow2 = 0;
/* beginning of the outer loop. */
L30:
jeval = TRUE_;
/* calculate the jacobian matrix. */
iflag = fdjac1(fcn, p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac,
ml, mu, epsfcn, &wa1[1], &wa2[1]);
*nfev += msum;
if (iflag < 0) {
goto L300;
}
/* compute the qr factorization of the jacobian. */
qrfac(n, n, &fjac[fjac_offset], ldfjac, FALSE_, iwa, 1, &wa1[1], &
wa2[1], &wa3[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) {
goto L70;
}
if (mode == 2) {
goto L50;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.) {
diag[j] = 1.;
}
/* L40: */
}
L50:
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa3[j] = diag[j] * x[j];
/* L60: */
}
xnorm = enorm(n, &wa3[1]);
delta = factor * xnorm;
if (delta == 0.) {
delta = factor;
}
L70:
/* form (q transpose)*fvec and store in qtf. */
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
qtf[i__] = fvec[i__];
/* L80: */
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
if (fjac[j + j * fjac_dim1] == 0.) {
goto L110;
}
sum = 0.;
i__2 = n;
for (i__ = j; i__ <= i__2; ++i__) {
sum += fjac[i__ + j * fjac_dim1] * qtf[i__];
/* L90: */
}
temp = -sum / fjac[j + j * fjac_dim1];
i__2 = n;
for (i__ = j; i__ <= i__2; ++i__) {
qtf[i__] += fjac[i__ + j * fjac_dim1] * temp;
/* L100: */
}
L110:
/* L120: */
;
}
/* copy the triangular factor of the qr factorization into r. */
sing = FALSE_;
i__1 = n;
for (j = 1; j <= i__1; ++j) {
l = j;
jm1 = j - 1;
if (jm1 < 1) {
goto L140;
}
i__2 = jm1;
for (i__ = 1; i__ <= i__2; ++i__) {
r__[l] = fjac[i__ + j * fjac_dim1];
l = l + n - i__;
/* L130: */
}
L140:
r__[l] = wa1[j];
if (wa1[j] == 0.) {
sing = TRUE_;
}
/* L150: */
}
/* accumulate the orthogonal factor in fjac. */
qform(n, n, &fjac[fjac_offset], ldfjac, &wa1[1]);
/* rescale if necessary. */
if (mode == 2) {
goto L170;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
/* Computing MAX */
d__1 = diag[j], d__2 = wa2[j];
diag[j] = max(d__1,d__2);
/* L160: */
}
L170:
/* beginning of the inner loop. */
L180:
/* if requested, call fcn to enable printing of iterates. */
if (nprint <= 0) {
goto L190;
}
iflag = 0;
if ((iter - 1) % nprint == 0) {
iflag = (*fcn)(p, n, &x[1], &fvec[1], 0);
}
if (iflag < 0) {
goto L300;
}
L190:
/* determine the direction p. */
dogleg(n, &r__[1], lr, &diag[1], &qtf[1], delta, &wa1[1], &wa2[1], &wa3[
1]);
/* store the direction p and x + p. calculate the norm of p. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
/* L200: */
}
pnorm = enorm(n, &wa3[1]);
/* on the first iteration, adjust the initial step bound. */
if (iter == 1) {
delta = min(delta,pnorm);
}
/* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, n, &wa2[1], &wa4[1], 1);
++(*nfev);
if (iflag < 0) {
goto L300;
}
fnorm1 = enorm(n, &wa4[1]);
/* compute the scaled actual reduction. */
actred = -1.;
if (fnorm1 < fnorm) {
/* Computing 2nd power */
d__1 = fnorm1 / fnorm;
actred = 1. - d__1 * d__1;
}
/* compute the scaled predicted reduction. */
l = 1;
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
sum = 0.;
i__2 = n;
for (j = i__; j <= i__2; ++j) {
sum += r__[l] * wa1[j];
++l;
/* L210: */
}
wa3[i__] = qtf[i__] + sum;
/* L220: */
}
temp = enorm(n, &wa3[1]);
prered = 0.;
if (temp < fnorm) {
/* Computing 2nd power */
d__1 = temp / fnorm;
prered = 1. - d__1 * d__1;
}
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered > 0.) {
ratio = actred / prered;
}
/* update the step bound. */
if (ratio >= p1) {
goto L230;
}
ncsuc = 0;
++ncfail;
delta = p5 * delta;
goto L240;
L230:
ncfail = 0;
++ncsuc;
if (ratio >= p5 || ncsuc > 1) {
/* Computing MAX */
d__1 = delta, d__2 = pnorm / p5;
delta = max(d__1,d__2);
}
if (fabs(ratio - 1.) <= p1) {
delta = pnorm / p5;
}
L240:
/* test for successful iteration. */
if (ratio < p0001) {
goto L260;
}
/* successful iteration. update x, fvec, and their norms. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
fvec[j] = wa4[j];
/* L250: */
}
xnorm = enorm(n, &wa2[1]);
fnorm = fnorm1;
++iter;
L260:
/* determine the progress of the iteration. */
++nslow1;
if (actred >= p001) {
nslow1 = 0;
}
if (jeval) {
++nslow2;
}
if (actred >= p1) {
nslow2 = 0;
}
/* test for convergence. */
if (delta <= xtol * xnorm || fnorm == 0.) {
info = 1;
}
if (info != 0) {
goto L300;
}
/* tests for termination and stringent tolerances. */
if (*nfev >= maxfev) {
info = 2;
}
/* Computing MAX */
d__1 = p1 * delta;
if (p1 * max(d__1,pnorm) <= epsmch * xnorm) {
info = 3;
}
if (nslow2 == 5) {
info = 4;
}
if (nslow1 == 10) {
info = 5;
}
if (info != 0) {
goto L300;
}
/* criterion for recalculating jacobian approximation */
/* by forward differences. */
if (ncfail == 2) {
goto L290;
}
/* calculate the rank one modification to the jacobian */
/* and update qtf if necessary. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
sum = 0.;
i__2 = n;
for (i__ = 1; i__ <= i__2; ++i__) {
sum += fjac[i__ + j * fjac_dim1] * wa4[i__];
/* L270: */
}
wa2[j] = (sum - wa3[j]) / pnorm;
wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm);
if (ratio >= p0001) {
qtf[j] = sum;
}
/* L280: */
}
/* compute the qr factorization of the updated jacobian. */
r1updt(n, n, &r__[1], lr, &wa1[1], &wa2[1], &wa3[1], &sing);
r1mpyq(n, n, &fjac[fjac_offset], ldfjac, &wa2[1], &wa3[1]);
r1mpyq(1, n, &qtf[1], 1, &wa2[1], &wa3[1]);
/* end of the inner loop. */
jeval = FALSE_;
goto L180;
L290:
/* end of the outer loop. */
goto L30;
L300:
/* termination, either normal or user imposed. */
if (iflag < 0) {
info = iflag;
}
if (nprint > 0) {
(*fcn)(p, n, &x[1], &fvec[1], 0);
}
return info;
/* last card of subroutine hybrd. */
} /* hybrd_ */

63
unsupported/Eigen/src/NonLinear/hybrd1.h Normal file
View File

@ -0,0 +1,63 @@
template<typename T>
int hybrd1_template(minpack_func_nn fcn, void *p, int n, T *x, T *
fvec, T tol, T *wa, int lwa)
{
/* Initialized data */
const T factor = 100.;
/* System generated locals */
int i__1;
/* Local variables */
int j, ml, lr, mu, mode, nfev;
T xtol;
int index;
T epsfcn;
int maxfev, nprint;
int info;
/* Parameter adjustments */
--fvec;
--x;
--wa;
/* Function Body */
info = 0;
/* check the input parameters for errors. */
if (n <= 0 || tol < 0. || lwa < n * (n * 3 + 13) / 2) {
/* goto L20; */
return info;
}
/* call hybrd. */
maxfev = (n + 1) * 200;
xtol = tol;
ml = n - 1;
mu = n - 1;
epsfcn = 0.;
mode = 2;
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa[j] = 1.;
/* L10: */
}
nprint = 0;
lr = n * (n + 1) / 2;
index = n * 6 + lr;
info = hybrd(fcn, p, n, &x[1], &fvec[1], xtol, maxfev, ml, mu, epsfcn, &
wa[1], mode, factor, nprint, &nfev, &wa[index + 1], n, &
wa[n * 6 + 1], lr, &wa[n + 1], &wa[(n << 1) + 1], &wa[n * 3
+ 1], &wa[(n << 2) + 1], &wa[n * 5 + 1]);
if (info == 5) {
info = 4;
}
/* L20: */
return info;
}

446
unsupported/Eigen/src/NonLinear/hybrj.h Normal file
View File

@ -0,0 +1,446 @@
template<typename T>
int hybrj_template(minpack_funcder_nn fcn, void *p, int n, T *x, T *
fvec, T *fjac, int ldfjac, T xtol, int
maxfev, T *diag, int mode, T factor, int
nprint, int *nfev, int *njev, T *r__,
int lr, T *qtf, T *wa1, T *wa2,
T *wa3, T *wa4)
{
/* Initialized data */
/* System generated locals */
int fjac_dim1, fjac_offset, i__1, i__2;
T d__1, d__2;
/* Local variables */
int i__, j, l, jm1, iwa[1];
T sum;
int sing;
int iter;
T temp;
int iflag;
T delta;
int jeval;
int ncsuc;
T ratio;
T fnorm;
T pnorm, xnorm, fnorm1;
int nslow1, nslow2;
int ncfail;
T actred, epsmch, prered;
int info;
/* Parameter adjustments */
--wa4;
--wa3;
--wa2;
--wa1;
--qtf;
--diag;
--fvec;
--x;
fjac_dim1 = ldfjac;
fjac_offset = 1 + fjac_dim1 * 1;
fjac -= fjac_offset;
--r__;
/* Function Body */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
info = 0;
iflag = 0;
*nfev = 0;
*njev = 0;
/* check the input parameters for errors. */
if (n <= 0 || ldfjac < n || xtol < 0. || maxfev <= 0 || factor <=
0. || lr < n * (n + 1) / 2) {
goto L300;
}
if (mode != 2) {
goto L20;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
if (diag[j] <= 0.) {
goto L300;
}
/* L10: */
}
L20:
/* evaluate the function at the starting point */
/* and calculate its norm. */
iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 1);
*nfev = 1;
if (iflag < 0) {
goto L300;
}
fnorm = enorm(n, &fvec[1]);
/* initialize iteration counter and monitors. */
iter = 1;
ncsuc = 0;
ncfail = 0;
nslow1 = 0;
nslow2 = 0;
/* beginning of the outer loop. */
L30:
jeval = TRUE_;
/* calculate the jacobian matrix. */
iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 2);
++(*njev);
if (iflag < 0) {
goto L300;
}
/* compute the qr factorization of the jacobian. */
qrfac(n, n, &fjac[fjac_offset], ldfjac, FALSE_, iwa, 1, &wa1[1], &
wa2[1], &wa3[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) {
goto L70;
}
if (mode == 2) {
goto L50;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.) {
diag[j] = 1.;
}
/* L40: */
}
L50:
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa3[j] = diag[j] * x[j];
/* L60: */
}
xnorm = enorm(n, &wa3[1]);
delta = factor * xnorm;
if (delta == 0.) {
delta = factor;
}
L70:
/* form (q transpose)*fvec and store in qtf. */
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
qtf[i__] = fvec[i__];
/* L80: */
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
if (fjac[j + j * fjac_dim1] == 0.) {
goto L110;
}
sum = 0.;
i__2 = n;
for (i__ = j; i__ <= i__2; ++i__) {
sum += fjac[i__ + j * fjac_dim1] * qtf[i__];
/* L90: */
}
temp = -sum / fjac[j + j * fjac_dim1];
i__2 = n;
for (i__ = j; i__ <= i__2; ++i__) {
qtf[i__] += fjac[i__ + j * fjac_dim1] * temp;
/* L100: */
}
L110:
/* L120: */
;
}
/* copy the triangular factor of the qr factorization into r. */
sing = FALSE_;
i__1 = n;
for (j = 1; j <= i__1; ++j) {
l = j;
jm1 = j - 1;
if (jm1 < 1) {
goto L140;
}
i__2 = jm1;
for (i__ = 1; i__ <= i__2; ++i__) {
r__[l] = fjac[i__ + j * fjac_dim1];
l = l + n - i__;
/* L130: */
}
L140:
r__[l] = wa1[j];
if (wa1[j] == 0.) {
sing = TRUE_;
}
/* L150: */
}
/* accumulate the orthogonal factor in fjac. */
qform(n, n, &fjac[fjac_offset], ldfjac, &wa1[1]);
/* rescale if necessary. */
if (mode == 2) {
goto L170;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
/* Computing MAX */
d__1 = diag[j], d__2 = wa2[j];
diag[j] = max(d__1,d__2);
/* L160: */
}
L170:
/* beginning of the inner loop. */
L180:
/* if requested, call fcn to enable printing of iterates. */
if (nprint <= 0) {
goto L190;
}
iflag = 0;
if ((iter - 1) % nprint == 0) {
iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 0);
}
if (iflag < 0) {
goto L300;
}
L190:
/* determine the direction p. */
dogleg(n, &r__[1], lr, &diag[1], &qtf[1], delta, &wa1[1], &wa2[1], &wa3[
1]);
/* store the direction p and x + p. calculate the norm of p. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
/* L200: */
}
pnorm = enorm(n, &wa3[1]);
/* on the first iteration, adjust the initial step bound. */
if (iter == 1) {
delta = min(delta,pnorm);
}
/* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, n, &wa2[1], &wa4[1], &fjac[fjac_offset], ldfjac, 1);
++(*nfev);
if (iflag < 0) {
goto L300;
}
fnorm1 = enorm(n, &wa4[1]);
/* compute the scaled actual reduction. */
actred = -1.;
if (fnorm1 < fnorm) {
/* Computing 2nd power */
d__1 = fnorm1 / fnorm;
actred = 1. - d__1 * d__1;
}
/* compute the scaled predicted reduction. */
l = 1;
i__1 = n;
for (i__ = 1; i__ <= i__1; ++i__) {
sum = 0.;
i__2 = n;
for (j = i__; j <= i__2; ++j) {
sum += r__[l] * wa1[j];
++l;
/* L210: */
}
wa3[i__] = qtf[i__] + sum;
/* L220: */
}
temp = enorm(n, &wa3[1]);
prered = 0.;
if (temp < fnorm) {
/* Computing 2nd power */
d__1 = temp / fnorm;
prered = 1. - d__1 * d__1;
}
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered > 0.) {
ratio = actred / prered;
}
/* update the step bound. */
if (ratio >= p1) {
goto L230;
}
ncsuc = 0;
++ncfail;
delta = p5 * delta;
goto L240;
L230:
ncfail = 0;
++ncsuc;
if (ratio >= p5 || ncsuc > 1) {
/* Computing MAX */
d__1 = delta, d__2 = pnorm / p5;
delta = max(d__1,d__2);
}
if (fabs(ratio - 1.) <= p1) {
delta = pnorm / p5;
}
L240:
/* test for successful iteration. */
if (ratio < p0001) {
goto L260;
}
/* successful iteration. update x, fvec, and their norms. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
fvec[j] = wa4[j];
/* L250: */
}
xnorm = enorm(n, &wa2[1]);
fnorm = fnorm1;
++iter;
L260:
/* determine the progress of the iteration. */
++nslow1;
if (actred >= p001) {
nslow1 = 0;
}
if (jeval) {
++nslow2;
}
if (actred >= p1) {
nslow2 = 0;
}
/* test for convergence. */
if (delta <= xtol * xnorm || fnorm == 0.) {
info = 1;
}
if (info != 0) {
goto L300;
}
/* tests for termination and stringent tolerances. */
if (*nfev >= maxfev) {
info = 2;
}
/* Computing MAX */
d__1 = p1 * delta;
if (p1 * max(d__1,pnorm) <= epsmch * xnorm) {
info = 3;
}
if (nslow2 == 5) {
info = 4;
}
if (nslow1 == 10) {
info = 5;
}
if (info != 0) {
goto L300;
}
/* criterion for recalculating jacobian. */
if (ncfail == 2) {
goto L290;
}
/* calculate the rank one modification to the jacobian */
/* and update qtf if necessary. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
sum = 0.;
i__2 = n;
for (i__ = 1; i__ <= i__2; ++i__) {
sum += fjac[i__ + j * fjac_dim1] * wa4[i__];
/* L270: */
}
wa2[j] = (sum - wa3[j]) / pnorm;
wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm);
if (ratio >= p0001) {
qtf[j] = sum;
}
/* L280: */
}
/* compute the qr factorization of the updated jacobian. */
r1updt(n, n, &r__[1], lr, &wa1[1], &wa2[1], &wa3[1], &sing);
r1mpyq(n, n, &fjac[fjac_offset], ldfjac, &wa2[1], &wa3[1]);
r1mpyq(1, n, &qtf[1], 1, &wa2[1], &wa3[1]);
/* end of the inner loop. */
jeval = FALSE_;
goto L180;
L290:
/* end of the outer loop. */
goto L30;
L300:
/* termination, either normal or user imposed. */
if (iflag < 0) {
info = iflag;
}
if (nprint > 0) {
iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 0);
}
return info;
/* last card of subroutine hybrj. */
} /* hybrj_ */

63
unsupported/Eigen/src/NonLinear/hybrj1.h Normal file
View File

@ -0,0 +1,63 @@
template<typename T>
int hybrj1_template(minpack_funcder_nn fcn, void *p, int n, T *x, T *
fvec, T *fjac, int ldfjac, T tol,
T *wa, int lwa)
{
/* Initialized data */
const T factor = 100.;
/* System generated locals */
int fjac_dim1, fjac_offset, i__1;
/* Local variables */
int j, lr, mode, nfev, njev;
T xtol;
int maxfev, nprint;
int info;
/* Parameter adjustments */
--fvec;
--x;
fjac_dim1 = ldfjac;
fjac_offset = 1 + fjac_dim1 * 1;
fjac -= fjac_offset;
--wa;
/* Function Body */
info = 0;
/* check the input parameters for errors. */
if (n <= 0 || ldfjac < n || tol < 0. || lwa < n * (n + 13) / 2) {
/* goto L20; */
return info;
}
/* call hybrj. */
maxfev = (n + 1) * 100;
xtol = tol;
mode = 2;
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa[j] = 1.;
/* L10: */
}
nprint = 0;
lr = n * (n + 1) / 2;
info = hybrj(fcn, p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, xtol,
maxfev, &wa[1], mode, factor, nprint, &nfev, &njev, &wa[
n * 6 + 1], lr, &wa[n + 1], &wa[(n << 1) + 1], &wa[n * 3 + 1],
&wa[(n << 2) + 1], &wa[n * 5 + 1]);
if (info == 5) {
info = 4;
}
/* L20: */
return info;
/* last card of subroutine hybrj1. */
} /* hybrj1_ */

423
unsupported/Eigen/src/NonLinear/lmder.h Normal file
View File

@ -0,0 +1,423 @@
template<typename T>
int lmder_template(minpack_funcder_mn fcn, void *p, int m, int n, T *x,
T *fvec, T *fjac, int ldfjac, T ftol,
T xtol, T gtol, int maxfev, T *
diag, int mode, T factor, int nprint,
int *nfev, int *njev, int *ipvt, T *qtf,
T *wa1, T *wa2, T *wa3, T *wa4)
{
/* Initialized data */
/* System generated locals */
int fjac_dim1, fjac_offset, i__1, i__2;
T d__1, d__2, d__3;
/* Local variables */
int i__, j, l;
T par, sum;
int iter;
T temp, temp1, temp2;
int iflag;
T delta;
T ratio;
T fnorm, gnorm, pnorm, xnorm, fnorm1, actred, dirder,
epsmch, prered;
int info;
/* Parameter adjustments */
--wa4;
--fvec;
--wa3;
--wa2;
--wa1;
--qtf;
--ipvt;
--diag;
--x;
fjac_dim1 = ldfjac;
fjac_offset = 1 + fjac_dim1 * 1;
fjac -= fjac_offset;
/* Function Body */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
info = 0;
iflag = 0;
*nfev = 0;
*njev = 0;
/* check the input parameters for errors. */
if (n <= 0 || m < n || ldfjac < m || ftol < 0. || xtol < 0. ||
gtol < 0. || maxfev <= 0 || factor <= 0.) {
goto L300;
}
if (mode != 2) {
goto L20;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
if (diag[j] <= 0.) {
goto L300;
}
/* L10: */
}
L20:
/* evaluate the function at the starting point */
/* and calculate its norm. */
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 1);
*nfev = 1;
if (iflag < 0) {
goto L300;
}
fnorm = enorm(m, &fvec[1]);
/* initialize levenberg-marquardt parameter and iteration counter. */
par = 0.;
iter = 1;
/* beginning of the outer loop. */
L30:
/* calculate the jacobian matrix. */
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 2);
++(*njev);
if (iflag < 0) {
goto L300;
}
/* if requested, call fcn 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], &fjac[fjac_offset], ldfjac, 0);
}
if (iflag < 0) {
goto L300;
}
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]);
/* on the first iteration and if mode is 1, scale according */
/* to the norms of the columns of the initial jacobian. */
if (iter != 1) {
goto L80;
}
if (mode == 2) {
goto L60;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.) {
diag[j] = 1.;
}
/* L50: */
}
L60:
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa3[j] = diag[j] * x[j];
/* L70: */
}
xnorm = enorm(n, &wa3[1]);
delta = factor * xnorm;
if (delta == 0.) {
delta = factor;
}
L80:
/* form (q transpose)*fvec and store the first n components in */
/* qtf. */
i__1 = m;
for (i__ = 1; i__ <= i__1; ++i__) {
wa4[i__] = fvec[i__];
/* L90: */
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
if (fjac[j + j * fjac_dim1] == 0.) {
goto L120;
}
sum = 0.;
i__2 = m;
for (i__ = j; i__ <= i__2; ++i__) {
sum += fjac[i__ + j * fjac_dim1] * wa4[i__];
/* L100: */
}
temp = -sum / fjac[j + j * fjac_dim1];
i__2 = m;
for (i__ = j; i__ <= i__2; ++i__) {
wa4[i__] += fjac[i__ + j * fjac_dim1] * temp;
/* L110: */
}
L120:
fjac[j + j * fjac_dim1] = wa1[j];
qtf[j] = wa4[j];
/* L130: */
}
/* compute the norm of the scaled gradient. */
gnorm = 0.;
if (fnorm == 0.) {
goto L170;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
l = ipvt[j];
if (wa2[l] == 0.) {
goto L150;
}
sum = 0.;
i__2 = j;
for (i__ = 1; i__ <= i__2; ++i__) {
sum += fjac[i__ + j * fjac_dim1] * (qtf[i__] / fnorm);
/* L140: */
}
/* Computing MAX */
d__2 = gnorm, d__3 = fabs(sum / wa2[l]);
gnorm = max(d__2,d__3);
L150:
/* L160: */
;
}
L170:
/* test for convergence of the gradient norm. */
if (gnorm <= gtol) {
info = 4;
}
if (info != 0) {
goto L300;
}
/* rescale if necessary. */
if (mode == 2) {
goto L190;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
/* Computing MAX */
d__1 = diag[j], d__2 = wa2[j];
diag[j] = max(d__1,d__2);
/* L180: */
}
L190:
/* beginning of the inner loop. */
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]);
/* store the direction p and x + p. calculate the norm of p. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
/* L210: */
}
pnorm = enorm(n, &wa3[1]);
/* on the first iteration, adjust the initial step bound. */
if (iter == 1) {
delta = min(delta,pnorm);
}
/* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, m, n, &wa2[1], &wa4[1], &fjac[fjac_offset], ldfjac, 1);
++(*nfev);
if (iflag < 0) {
goto L300;
}
fnorm1 = enorm(m, &wa4[1]);
/* compute the scaled actual reduction. */
actred = -1.;
if (p1 * fnorm1 < fnorm) {
/* Computing 2nd power */
d__1 = fnorm1 / fnorm;
actred = 1. - d__1 * d__1;
}
/* compute the scaled predicted reduction and */
/* the scaled directional derivative. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa3[j] = 0.;
l = ipvt[j];
temp = wa1[l];
i__2 = j;
for (i__ = 1; i__ <= i__2; ++i__) {
wa3[i__] += fjac[i__ + j * fjac_dim1] * temp;
/* L220: */
}
/* L230: */
}
temp1 = enorm(n, &wa3[1]) / fnorm;
temp2 = sqrt(par) * pnorm / fnorm;
/* Computing 2nd power */
d__1 = temp1;
/* Computing 2nd power */
d__2 = temp2;
prered = d__1 * d__1 + d__2 * d__2 / p5;
/* Computing 2nd power */
d__1 = temp1;
/* Computing 2nd power */
d__2 = temp2;
dirder = -(d__1 * d__1 + d__2 * d__2);
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered != 0.) {
ratio = actred / prered;
}
/* update the step bound. */
if (ratio > p25) {
goto L240;
}
if (actred >= 0.) {
temp = p5;
}
if (actred < 0.) {
temp = p5 * dirder / (dirder + p5 * actred);
}
if (p1 * fnorm1 >= fnorm || temp < p1) {
temp = p1;
}
/* Computing MIN */
d__1 = delta, d__2 = pnorm / p1;
delta = temp * min(d__1,d__2);
par /= temp;
goto L260;
L240:
if (par != 0. && ratio < p75) {
goto L250;
}
delta = pnorm / p5;
par = p5 * par;
L250:
L260:
/* test for successful iteration. */
if (ratio < p0001) {
goto L290;
}
/* successful iteration. update x, fvec, and their norms. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
/* L270: */
}
i__1 = m;
for (i__ = 1; i__ <= i__1; ++i__) {
fvec[i__] = wa4[i__];
/* L280: */
}
xnorm = enorm(n, &wa2[1]);
fnorm = fnorm1;
++iter;
L290:
/* tests for convergence. */
if (fabs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1.) {
info = 1;
}
if (delta <= xtol * xnorm) {
info = 2;
}
if (fabs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1. && info
== 2) {
info = 3;
}
if (info != 0) {
goto L300;
}
/* tests for termination and stringent tolerances. */
if (*nfev >= maxfev) {
info = 5;
}
if (fabs(actred) <= epsmch && prered <= epsmch && p5 * ratio <= 1.) {
info = 6;
}
if (delta <= epsmch * xnorm) {
info = 7;
}
if (gnorm <= epsmch) {
info = 8;
}
if (info != 0) {
goto L300;
}
/* end of the inner loop. repeat if iteration unsuccessful. */
if (ratio < p0001) {
goto L200;
}
/* end of the outer loop. */
goto L30;
L300:
/* termination, either normal or user imposed. */
if (iflag < 0) {
info = iflag;
}
iflag = 0;
if (nprint > 0) {
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 0);
}
return info;
/* last card of subroutine lmder. */
} /* lmder_ */

63
unsupported/Eigen/src/NonLinear/lmder1.h Normal file
View File

@ -0,0 +1,63 @@
template<typename T>
int lmder1_template(minpack_funcder_mn fcn, void *p, int m, int n, T *x,
T *fvec, T *fjac, int ldfjac, T tol,
int *ipvt, T *wa, int lwa)
{
/* Initialized data */
const T factor = 100.;
/* System generated locals */
int fjac_dim1, fjac_offset;
/* Local variables */
int mode, nfev, njev;
T ftol, gtol, xtol;
int maxfev, nprint;
int info;
/* Parameter adjustments */
--fvec;
--ipvt;
--x;
fjac_dim1 = ldfjac;
fjac_offset = 1 + fjac_dim1 * 1;
fjac -= fjac_offset;
--wa;
/* Function Body */
info = 0;
/* check the input parameters for errors. */
if (n <= 0 || m < n || ldfjac < m || tol < 0. || lwa < n * 5 +
m) {
/* goto L10; */
printf("lmder1 bad args : m,n,tol,...");
return info;
}
/* call lmder. */
maxfev = (n + 1) * 100;
ftol = tol;
xtol = tol;
gtol = 0.;
mode = 1;
nprint = 0;
info = lmder(fcn, p, m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac,
ftol, xtol, gtol, maxfev, &wa[1], mode, factor, nprint,
&nfev, &njev, &ipvt[1], &wa[n + 1], &wa[(n << 1) + 1], &
wa[n * 3 + 1], &wa[(n << 2) + 1], &wa[n * 5 + 1]);
if (info == 8) {
info = 4;
}
/* L10: */
return info;
/* last card of subroutine lmder1. */
} /* lmder1_ */

425
unsupported/Eigen/src/NonLinear/lmdif.h Normal file
View File

@ -0,0 +1,425 @@
template<typename T>
int lmdif_template(minpack_func_mn fcn, void *p, int m, int n, T *x,
T *fvec, T ftol, T xtol, T
gtol, int maxfev, T epsfcn, T *diag, int
mode, T factor, int nprint, int *
nfev, T *fjac, int ldfjac, int *ipvt, T *
qtf, T *wa1, T *wa2, T *wa3, T *
wa4)
{
/* Initialized data */
/* System generated locals */
int fjac_dim1, fjac_offset, i__1, i__2;
T d__1, d__2, d__3;
/* Local variables */
int i__, j, l;
T par, sum;
int iter;
T temp, temp1, temp2;
int iflag;
T delta;
T ratio;
T fnorm, gnorm;
T pnorm, xnorm, fnorm1, actred, dirder, epsmch, prered;
int info;
/* Parameter adjustments */
--wa4;
--fvec;
--wa3;
--wa2;
--wa1;
--qtf;
--ipvt;
--diag;
--x;
fjac_dim1 = ldfjac;
fjac_offset = 1 + fjac_dim1 * 1;
fjac -= fjac_offset;
/* Function Body */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
info = 0;
iflag = 0;
*nfev = 0;
/* check the input parameters for errors. */
if (n <= 0 || m < n || ldfjac < m || ftol < 0. || xtol < 0. ||
gtol < 0. || maxfev <= 0 || factor <= 0.) {
goto L300;
}
if (mode != 2) {
goto L20;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
if (diag[j] <= 0.) {
goto L300;
}
/* L10: */
}
L20:
/* evaluate the function at the starting point */
/* and calculate its norm. */
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], 1);
*nfev = 1;
if (iflag < 0) {
goto L300;
}
fnorm = enorm(m, &fvec[1]);
/* initialize levenberg-marquardt parameter and iteration counter. */
par = 0.;
iter = 1;
/* beginning of the outer loop. */
L30:
/* calculate the jacobian matrix. */
iflag = fdjac2(fcn, p, m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac,
epsfcn, &wa4[1]);
*nfev += n;
if (iflag < 0) {
goto L300;
}
/* if requested, call fcn 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);
}
if (iflag < 0) {
goto L300;
}
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]);
/* on the first iteration and if mode is 1, scale according */
/* to the norms of the columns of the initial jacobian. */
if (iter != 1) {
goto L80;
}
if (mode == 2) {
goto L60;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.) {
diag[j] = 1.;
}
/* L50: */
}
L60:
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa3[j] = diag[j] * x[j];
/* L70: */
}
xnorm = enorm(n, &wa3[1]);
delta = factor * xnorm;
if (delta == 0.) {
delta = factor;
}
L80:
/* form (q transpose)*fvec and store the first n components in */
/* qtf. */
i__1 = m;
for (i__ = 1; i__ <= i__1; ++i__) {
wa4[i__] = fvec[i__];
/* L90: */
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
if (fjac[j + j * fjac_dim1] == 0.) {
goto L120;
}
sum = 0.;
i__2 = m;
for (i__ = j; i__ <= i__2; ++i__) {
sum += fjac[i__ + j * fjac_dim1] * wa4[i__];
/* L100: */
}
temp = -sum / fjac[j + j * fjac_dim1];
i__2 = m;
for (i__ = j; i__ <= i__2; ++i__) {
wa4[i__] += fjac[i__ + j * fjac_dim1] * temp;
/* L110: */
}
L120:
fjac[j + j * fjac_dim1] = wa1[j];
qtf[j] = wa4[j];
/* L130: */
}
/* compute the norm of the scaled gradient. */
gnorm = 0.;
if (fnorm == 0.) {
goto L170;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
l = ipvt[j];
if (wa2[l] == 0.) {
goto L150;
}
sum = 0.;
i__2 = j;
for (i__ = 1; i__ <= i__2; ++i__) {
sum += fjac[i__ + j * fjac_dim1] * (qtf[i__] / fnorm);
/* L140: */
}
/* Computing MAX */
d__2 = gnorm, d__3 = fabs(sum / wa2[l]);
gnorm = max(d__2,d__3);
L150:
/* L160: */
;
}
L170:
/* test for convergence of the gradient norm. */
if (gnorm <= gtol) {
info = 4;
}
if (info != 0) {
goto L300;
}
/* rescale if necessary. */
if (mode == 2) {
goto L190;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
/* Computing MAX */
d__1 = diag[j], d__2 = wa2[j];
diag[j] = max(d__1,d__2);
/* L180: */
}
L190:
/* beginning of the inner loop. */
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]);
/* store the direction p and x + p. calculate the norm of p. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
/* L210: */
}
pnorm = enorm(n, &wa3[1]);
/* on the first iteration, adjust the initial step bound. */
if (iter == 1) {
delta = min(delta,pnorm);
}
/* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, m, n, &wa2[1], &wa4[1], 1);
++(*nfev);
if (iflag < 0) {
goto L300;
}
fnorm1 = enorm(m, &wa4[1]);
/* compute the scaled actual reduction. */
actred = -1.;
if (p1 * fnorm1 < fnorm) {
/* Computing 2nd power */
d__1 = fnorm1 / fnorm;
actred = 1. - d__1 * d__1;
}
/* compute the scaled predicted reduction and */
/* the scaled directional derivative. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa3[j] = 0.;
l = ipvt[j];
temp = wa1[l];
i__2 = j;
for (i__ = 1; i__ <= i__2; ++i__) {
wa3[i__] += fjac[i__ + j * fjac_dim1] * temp;
/* L220: */
}
/* L230: */
}
temp1 = enorm(n, &wa3[1]) / fnorm;
temp2 = sqrt(par) * pnorm / fnorm;
/* Computing 2nd power */
d__1 = temp1;
/* Computing 2nd power */
d__2 = temp2;
prered = d__1 * d__1 + d__2 * d__2 / p5;
/* Computing 2nd power */
d__1 = temp1;
/* Computing 2nd power */
d__2 = temp2;
dirder = -(d__1 * d__1 + d__2 * d__2);
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered != 0.) {
ratio = actred / prered;
}
/* update the step bound. */
if (ratio > p25) {
goto L240;
}
if (actred >= 0.) {
temp = p5;
}
if (actred < 0.) {
temp = p5 * dirder / (dirder + p5 * actred);
}
if (p1 * fnorm1 >= fnorm || temp < p1) {
temp = p1;
}
/* Computing MIN */
d__1 = delta, d__2 = pnorm / p1;
delta = temp * min(d__1,d__2);
par /= temp;
goto L260;
L240:
if (par != 0. && ratio < p75) {
goto L250;
}
delta = pnorm / p5;
par = p5 * par;
L250:
L260:
/* test for successful iteration. */
if (ratio < p0001) {
goto L290;
}
/* successful iteration. update x, fvec, and their norms. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
/* L270: */
}
i__1 = m;
for (i__ = 1; i__ <= i__1; ++i__) {
fvec[i__] = wa4[i__];
/* L280: */
}
xnorm = enorm(n, &wa2[1]);
fnorm = fnorm1;
++iter;
L290:
/* tests for convergence. */
if (fabs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1.) {
info = 1;
}
if (delta <= xtol * xnorm) {
info = 2;
}
if (fabs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1. && info
== 2) {
info = 3;
}
if (info != 0) {
goto L300;
}
/* tests for termination and stringent tolerances. */
if (*nfev >= maxfev) {
info = 5;
}
if (fabs(actred) <= epsmch && prered <= epsmch && p5 * ratio <= 1.) {
info = 6;
}
if (delta <= epsmch * xnorm) {
info = 7;
}
if (gnorm <= epsmch) {
info = 8;
}
if (info != 0) {
goto L300;
}
/* end of the inner loop. repeat if iteration unsuccessful. */
if (ratio < p0001) {
goto L200;
}
/* end of the outer loop. */
goto L30;
L300:
/* termination, either normal or user imposed. */
if (iflag < 0) {
info = iflag;
}
iflag = 0;
if (nprint > 0) {
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], 0);
}
return info;
/* last card of subroutine lmdif. */
} /* lmdif_ */

56
unsupported/Eigen/src/NonLinear/lmdif1.h Normal file
View File

@ -0,0 +1,56 @@
template<typename T>
int lmdif1_template(minpack_func_mn fcn, void *p, int m, int n, T *x,
T *fvec, T tol, int *iwa,
T *wa, int lwa)
{
/* Initialized data */
const T factor = 100.;
int mp5n, mode, nfev;
T ftol, gtol, xtol;
T epsfcn;
int maxfev, nprint;
int info;
/* Parameter adjustments */
--fvec;
--iwa;
--x;
--wa;
/* Function Body */
info = 0;
/* check the input parameters for errors. */
if (n <= 0 || m < n || tol < 0. || lwa < m * n + n * 5 + m) {
/* goto L10; */
return info;
}
/* call lmdif. */
maxfev = (n + 1) * 200;
ftol = tol;
xtol = tol;
gtol = 0.;
epsfcn = 0.;
mode = 1;
nprint = 0;
mp5n = m + n * 5;
info = lmdif(fcn, p, m, n, &x[1], &fvec[1], ftol, xtol, gtol, maxfev,
epsfcn, &wa[1], mode, factor, nprint, &nfev, &wa[mp5n +
1], m, &iwa[1], &wa[n + 1], &wa[(n << 1) + 1], &wa[n * 3 + 1],
&wa[(n << 2) + 1], &wa[n * 5 + 1]);
if (info == 8) {
info = 4;
}
/* L10: */
return info;
/* last card of subroutine lmdif1. */
} /* lmdif1_ */

450
unsupported/Eigen/src/NonLinear/lmstr.h Normal file
View File

@ -0,0 +1,450 @@
template<typename T>
int lmstr_template(minpack_funcderstr_mn fcn, void *p, int m, int n, T *x,
T *fvec, T *fjac, int ldfjac, T ftol,
T xtol, T gtol, int maxfev, T *
diag, int mode, T factor, int nprint,
int *nfev, int *njev, int *ipvt, T *qtf,
T *wa1, T *wa2, T *wa3, T *wa4)
{
/* Initialized data */
/* System generated locals */
int fjac_dim1, fjac_offset, i__1, i__2;
T d__1, d__2, d__3;
/* Local variables */
int i__, j, l;
T par, sum;
int sing;
int iter;
T temp, temp1, temp2;
int iflag;
T delta;
T ratio;
T fnorm, gnorm, pnorm, xnorm, fnorm1, actred, dirder,
epsmch, prered;
int info;
/* Parameter adjustments */
--wa4;
--fvec;
--wa3;
--wa2;
--wa1;
--qtf;
--ipvt;
--diag;
--x;
fjac_dim1 = ldfjac;
fjac_offset = 1 + fjac_dim1 * 1;
fjac -= fjac_offset;
/* Function Body */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
info = 0;
iflag = 0;
*nfev = 0;
*njev = 0;
/* check the input parameters for errors. */
if (n <= 0 || m < n || ldfjac < n || ftol < 0. || xtol < 0. ||
gtol < 0. || maxfev <= 0 || factor <= 0.) {
goto L340;
}
if (mode != 2) {
goto L20;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
if (diag[j] <= 0.) {
goto L340;
}
/* L10: */
}
L20:
/* evaluate the function at the starting point */
/* and calculate its norm. */
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], &wa3[1], 1);
*nfev = 1;
if (iflag < 0) {
goto L340;
}
fnorm = enorm(m, &fvec[1]);
/* initialize levenberg-marquardt parameter and iteration counter. */
par = 0.;
iter = 1;
/* beginning of the outer loop. */
L30:
/* if requested, call fcn 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], &wa3[1], 0);
}
if (iflag < 0) {
goto L340;
}
L40:
/* 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. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
qtf[j] = 0.;
i__2 = n;
for (i__ = 1; i__ <= i__2; ++i__) {
fjac[i__ + j * fjac_dim1] = 0.;
/* L50: */
}
/* L60: */
}
iflag = 2;
i__1 = m;
for (i__ = 1; i__ <= i__1; ++i__) {
if ((*fcn)(p, m, n, &x[1], &fvec[1], &wa3[1], iflag) < 0) {
goto L340;
}
temp = fvec[i__];
rwupdt(n, &fjac[fjac_offset], ldfjac, &wa3[1], &qtf[1], &temp, &wa1[
1], &wa2[1]);
++iflag;
/* L70: */
}
++(*njev);
/* if the jacobian is rank deficient, call qrfac to */
/* reorder its columns and update the components of qtf. */
sing = FALSE_;
i__1 = n;
for (j = 1; j <= i__1; ++j) {
if (fjac[j + j * fjac_dim1] == 0.) {
sing = TRUE_;
}
ipvt[j] = j;
wa2[j] = enorm(j, &fjac[j * fjac_dim1 + 1]);
/* L80: */
}
if (! sing) {
goto L130;
}
qrfac(n, n, &fjac[fjac_offset], ldfjac, TRUE_, &ipvt[1], n, &wa1[1], &
wa2[1], &wa3[1]);
i__1 = n;
for (j = 1; j <= i__1; ++j) {
if (fjac[j + j * fjac_dim1] == 0.) {
goto L110;
}
sum = 0.;
i__2 = n;
for (i__ = j; i__ <= i__2; ++i__) {
sum += fjac[i__ + j * fjac_dim1] * qtf[i__];
/* L90: */
}
temp = -sum / fjac[j + j * fjac_dim1];
i__2 = n;
for (i__ = j; i__ <= i__2; ++i__) {
qtf[i__] += fjac[i__ + j * fjac_dim1] * temp;
/* L100: */
}
L110:
fjac[j + j * fjac_dim1] = wa1[j];
/* L120: */
}
L130:
/* on the first iteration and if mode is 1, scale according */
/* to the norms of the columns of the initial jacobian. */
if (iter != 1) {
goto L170;
}
if (mode == 2) {
goto L150;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.) {
diag[j] = 1.;
}
/* L140: */
}
L150:
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa3[j] = diag[j] * x[j];
/* L160: */
}
xnorm = enorm(n, &wa3[1]);
delta = factor * xnorm;
if (delta == 0.) {
delta = factor;
}
L170:
/* compute the norm of the scaled gradient. */
gnorm = 0.;
if (fnorm == 0.) {
goto L210;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
l = ipvt[j];
if (wa2[l] == 0.) {
goto L190;
}
sum = 0.;
i__2 = j;
for (i__ = 1; i__ <= i__2; ++i__) {
sum += fjac[i__ + j * fjac_dim1] * (qtf[i__] / fnorm);
/* L180: */
}
/* Computing MAX */
d__2 = gnorm, d__3 = (d__1 = sum / wa2[l], abs(d__1));
gnorm = max(d__2,d__3);
L190:
/* L200: */
;
}
L210:
/* test for convergence of the gradient norm. */
if (gnorm <= gtol) {
info = 4;
}
if (info != 0) {
goto L340;
}
/* rescale if necessary. */
if (mode == 2) {
goto L230;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
/* Computing MAX */
d__1 = diag[j], d__2 = wa2[j];
diag[j] = max(d__1,d__2);
/* L220: */
}
L230:
/* beginning of the inner loop. */
L240:
/* 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]);
/* store the direction p and x + p. calculate the norm of p. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
/* L250: */
}
pnorm = enorm(n, &wa3[1]);
/* on the first iteration, adjust the initial step bound. */
if (iter == 1) {
delta = min(delta,pnorm);
}
/* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, m, n, &wa2[1], &wa4[1], &wa3[1], 1);
++(*nfev);
if (iflag < 0) {
goto L340;
}
fnorm1 = enorm(m, &wa4[1]);
/* compute the scaled actual reduction. */
actred = -1.;
if (p1 * fnorm1 < fnorm) {
/* Computing 2nd power */
d__1 = fnorm1 / fnorm;
actred = 1. - d__1 * d__1;
}
/* compute the scaled predicted reduction and */
/* the scaled directional derivative. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa3[j] = 0.;
l = ipvt[j];
temp = wa1[l];
i__2 = j;
for (i__ = 1; i__ <= i__2; ++i__) {
wa3[i__] += fjac[i__ + j * fjac_dim1] * temp;
/* L260: */
}
/* L270: */
}
temp1 = enorm(n, &wa3[1]) / fnorm;
temp2 = sqrt(par) * pnorm / fnorm;
/* Computing 2nd power */
d__1 = temp1;
/* Computing 2nd power */
d__2 = temp2;
prered = d__1 * d__1 + d__2 * d__2 / p5;
/* Computing 2nd power */
d__1 = temp1;
/* Computing 2nd power */
d__2 = temp2;
dirder = -(d__1 * d__1 + d__2 * d__2);
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered != 0.) {
ratio = actred / prered;
}
/* update the step bound. */
if (ratio > p25) {
goto L280;
}
if (actred >= 0.) {
temp = p5;
}
if (actred < 0.) {
temp = p5 * dirder / (dirder + p5 * actred);
}
if (p1 * fnorm1 >= fnorm || temp < p1) {
temp = p1;
}
/* Computing MIN */
d__1 = delta, d__2 = pnorm / p1;
delta = temp * min(d__1,d__2);
par /= temp;
goto L300;
L280:
if (par != 0. && ratio < p75) {
goto L290;
}
delta = pnorm / p5;
par = p5 * par;
L290:
L300:
/* test for successful iteration. */
if (ratio < p0001) {
goto L330;
}
/* successful iteration. update x, fvec, and their norms. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
/* L310: */
}
i__1 = m;
for (i__ = 1; i__ <= i__1; ++i__) {
fvec[i__] = wa4[i__];
/* L320: */
}
xnorm = enorm(n, &wa2[1]);
fnorm = fnorm1;
++iter;
L330:
/* tests for convergence. */
if (abs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1.) {
info = 1;
}
if (delta <= xtol * xnorm) {
info = 2;
}
if (abs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1. && info
== 2) {
info = 3;
}
if (info != 0) {
goto L340;
}
/* tests for termination and stringent tolerances. */
if (*nfev >= maxfev) {
info = 5;
}
if (abs(actred) <= epsmch && prered <= epsmch && p5 * ratio <= 1.) {
info = 6;
}
if (delta <= epsmch * xnorm) {
info = 7;
}
if (gnorm <= epsmch) {
info = 8;
}
if (info != 0) {
goto L340;
}
/* end of the inner loop. repeat if iteration unsuccessful. */
if (ratio < p0001) {
goto L240;
}
/* end of the outer loop. */
goto L30;
L340:
/* termination, either normal or user imposed. */
if (iflag < 0) {
info = iflag;
}
iflag = 0;
if (nprint > 0) {
iflag = (*fcn)(p, m, n, &x[1], &fvec[1], &wa3[1], 0);
}
return info;
/* last card of subroutine lmstr. */
} /* lmstr_ */

61
unsupported/Eigen/src/NonLinear/lmstr1.h Normal file
View File

@ -0,0 +1,61 @@
template<typename T>
int lmstr1_template(minpack_funcderstr_mn fcn, void *p, int m, int n, T *x,
T *fvec, T *fjac, int ldfjac, T tol,
int *ipvt, T *wa, int lwa)
{
/* Initialized data */
const T factor = 100.;
/* System generated locals */
int fjac_dim1, fjac_offset;
/* Local variables */
int mode, nfev, njev;
T ftol, gtol, xtol;
int maxfev, nprint;
int info;
/* Parameter adjustments */
--fvec;
--ipvt;
--x;
fjac_dim1 = ldfjac;
fjac_offset = 1 + fjac_dim1 * 1;
fjac -= fjac_offset;
--wa;
/* Function Body */
info = 0;
/* check the input parameters for errors. */
if (n <= 0 || m < n || ldfjac < n || tol < 0. || lwa < n * 5 +
m) {
/* goto L10; */
return info;
}
/* call lmstr. */
maxfev = (n + 1) * 100;
ftol = tol;
xtol = tol;
gtol = 0.;
mode = 1;
nprint = 0;
info = lmstr(fcn, p, m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac,
ftol, xtol, gtol, maxfev, &wa[1], mode, factor, nprint,
&nfev, &njev, &ipvt[1], &wa[n + 1], &wa[(n << 1) + 1], &
wa[n * 3 + 1], &wa[(n << 2) + 1], &wa[n * 5 + 1]);
if (info == 8) {
info = 4;
}
/* L10: */
return info;
/* last card of subroutine lmstr1. */
} /* lmstr1_ */

2
unsupported/test/CMakeLists.txt
View File

@ -22,5 +22,5 @@ LINK_LIBRARIES(/home/orzel/tmp/cminpack-1.0.2/libminpack.a)
ei_add_test(NonLinear)
ei_add_test(autodiff)
ei_add_test(BVH)
ei_add_test(matrixExponential)
#ei_add_test(matrixExponential)
ei_add_test(alignedvector3)