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

rename NonLinear to NonLinearOptimization

Browse Source
This commit is contained in:
Thomas Capricelli
2009-11-09 03:27:15 +01:00
parent 09cb27c587
commit 71a3e96b49
16 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
unsupported/Eigen/src/NonLinearOptimization/CMakeLists.txt Normal file
View File

@@ -0,0 +1,6 @@
FILE(GLOB Eigen_NonLinear_SRCS "*.h")
INSTALL(FILES
${Eigen_NonLinear_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/NonLinear COMPONENT Devel
)

725
unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h Normal file
View File

@@ -0,0 +1,725 @@
template<typename FunctorType, typename Scalar=double>
class HybridNonLinearSolver
{
public:
HybridNonLinearSolver(FunctorType &_functor)
: functor(_functor) { nfev=njev=iter = 0; fnorm= 0.; }
enum Status {
Running = -1,
ImproperInputParameters = 0,
RelativeErrorTooSmall = 1,
TooManyFunctionEvaluation = 2,
TolTooSmall = 3,
NotMakingProgressJacobian = 4,
NotMakingProgressIterations = 5,
UserAksed = 6
};
struct Parameters {
Parameters()
: factor(Scalar(100.))
, maxfev(1000)
, xtol(ei_sqrt(epsilon<Scalar>()))
, nb_of_subdiagonals(-1)
, nb_of_superdiagonals(-1)
, epsfcn(Scalar(0.)) {}
Scalar factor;
int maxfev; // maximum number of function evaluation
Scalar xtol;
int nb_of_subdiagonals;
int nb_of_superdiagonals;
Scalar epsfcn;
};
Status hybrj1(
Matrix< Scalar, Dynamic, 1 > &x,
const Scalar tol = ei_sqrt(epsilon<Scalar>())
);
Status solveInit(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
Status solveOneStep(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
Status solve(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
Status hybrd1(
Matrix< Scalar, Dynamic, 1 > &x,
const Scalar tol = ei_sqrt(epsilon<Scalar>())
);
Status solveNumericalDiffInit(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
Status solveNumericalDiffOneStep(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
Status solveNumericalDiff(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
void resetParameters(void) { parameters = Parameters(); }
Parameters parameters;
Matrix< Scalar, Dynamic, 1 > fvec;
Matrix< Scalar, Dynamic, Dynamic > fjac;
Matrix< Scalar, Dynamic, 1 > R;
Matrix< Scalar, Dynamic, 1 > qtf;
Matrix< Scalar, Dynamic, 1 > diag;
int nfev;
int njev;
int iter;
Scalar fnorm;
private:
FunctorType &functor;
int n;
Scalar sum;
bool sing;
Scalar temp;
Scalar delta;
bool jeval;
int ncsuc;
Scalar ratio;
Scalar pnorm, xnorm, fnorm1;
int nslow1, nslow2;
int ncfail;
Scalar actred, prered;
Matrix< Scalar, Dynamic, 1 > wa1, wa2, wa3, wa4;
};
template<typename FunctorType, typename Scalar>
typename HybridNonLinearSolver<FunctorType,Scalar>::Status
HybridNonLinearSolver<FunctorType,Scalar>::hybrj1(
Matrix< Scalar, Dynamic, 1 > &x,
const Scalar tol
)
{
n = x.size();
/* check the input parameters for errors. */
if (n <= 0 || tol < 0.)
return ImproperInputParameters;
resetParameters();
parameters.maxfev = 100*(n+1);
parameters.xtol = tol;
diag.setConstant(n, 1.);
return solve(
x,
2
);
}
template<typename FunctorType, typename Scalar>
typename HybridNonLinearSolver<FunctorType,Scalar>::Status
HybridNonLinearSolver<FunctorType,Scalar>::solveInit(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
n = x.size();
wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n);
fvec.resize(n);
qtf.resize(n);
R.resize( (n*(n+1))/2);
fjac.resize(n, n);
if (mode != 2)
diag.resize(n);
assert( (mode!=2 || diag.size()==n) || "When using mode==2, the caller must provide a valid 'diag'");
/* Function Body */
nfev = 0;
njev = 0;
/* check the input parameters for errors. */
if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0. )
return ImproperInputParameters;
if (mode == 2)
for (int j = 0; j < n; ++j)
if (diag[j] <= 0.)
return ImproperInputParameters;
/* evaluate the function at the starting point */
/* and calculate its norm. */
nfev = 1;
if ( functor(x, fvec) < 0)
return UserAksed;
fnorm = fvec.stableNorm();
/* initialize iteration counter and monitors. */
iter = 1;
ncsuc = 0;
ncfail = 0;
nslow1 = 0;
nslow2 = 0;
return Running;
}
template<typename FunctorType, typename Scalar>
typename HybridNonLinearSolver<FunctorType,Scalar>::Status
HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
int i, j, l, iwa[1];
jeval = true;
/* calculate the jacobian matrix. */
if ( functor.df(x, fjac) < 0)
return UserAksed;
++njev;
/* compute the qr factorization of the jacobian. */
ei_qrfac<Scalar>(n, n, fjac.data(), fjac.rows(), false, iwa, wa1.data(), wa2.data());
if (iter == 1) {
/* on the first iteration and if mode is 1, scale according */
/* to the norms of the columns of the initial jacobian. */
if (mode != 2)
for (j = 0; j < n; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.)
diag[j] = 1.;
}
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
wa3 = diag.cwise() * x;
xnorm = wa3.stableNorm();
delta = parameters.factor * xnorm;
if (delta == 0.)
delta = parameters.factor;
}
/* form (q transpose)*fvec and store in qtf. */
qtf = fvec;
for (j = 0; j < n; ++j)
if (fjac(j,j) != 0.) {
sum = 0.;
for (i = j; i < n; ++i)
sum += fjac(i,j) * qtf[i];
temp = -sum / fjac(j,j);
for (i = j; i < n; ++i)
qtf[i] += fjac(i,j) * temp;
}
/* copy the triangular factor of the qr factorization into r. */
sing = false;
for (j = 0; j < n; ++j) {
l = j;
if (j)
for (i = 0; i < j; ++i) {
R[l] = fjac(i,j);
l = l + n - i -1;
}
R[l] = wa1[j];
if (wa1[j] == 0.)
sing = true;
}
/* accumulate the orthogonal factor in fjac. */
ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data());
/* rescale if necessary. */
/* Computing MAX */
if (mode != 2)
diag = diag.cwise().max(wa2);
/* beginning of the inner loop. */
while (true) {
/* determine the direction p. */
ei_dogleg<Scalar>(R, diag, qtf, delta, wa1);
/* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1;
wa2 = x + wa1;
wa3 = diag.cwise() * wa1;
pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */
if (iter == 1)
delta = std::min(delta,pnorm);
/* evaluate the function at x + p and calculate its norm. */
if ( functor(wa2, wa4) < 0)
return UserAksed;
++nfev;
fnorm1 = wa4.stableNorm();
/* compute the scaled actual reduction. */
actred = -1.;
if (fnorm1 < fnorm) /* Computing 2nd power */
actred = 1. - ei_abs2(fnorm1 / fnorm);
/* compute the scaled predicted reduction. */
l = 0;
for (i = 0; i < n; ++i) {
sum = 0.;
for (j = i; j < n; ++j) {
sum += R[l] * wa1[j];
++l;
}
wa3[i] = qtf[i] + sum;
}
temp = wa3.stableNorm();
prered = 0.;
if (temp < fnorm) /* Computing 2nd power */
prered = 1. - ei_abs2(temp / fnorm);
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered > 0.)
ratio = actred / prered;
/* update the step bound. */
if (ratio < Scalar(.1)) {
ncsuc = 0;
++ncfail;
delta = Scalar(.5) * delta;
} else {
ncfail = 0;
++ncsuc;
if (ratio >= Scalar(.5) || ncsuc > 1) /* Computing MAX */
delta = std::max(delta, pnorm / Scalar(.5));
if (ei_abs(ratio - 1.) <= Scalar(.1)) {
delta = pnorm / Scalar(.5);
}
}
/* test for successful iteration. */
if (ratio >= Scalar(1e-4)) {
/* successful iteration. update x, fvec, and their norms. */
x = wa2;
wa2 = diag.cwise() * x;
fvec = wa4;
xnorm = wa2.stableNorm();
fnorm = fnorm1;
++iter;
}
/* determine the progress of the iteration. */
++nslow1;
if (actred >= Scalar(.001))
nslow1 = 0;
if (jeval)
++nslow2;
if (actred >= Scalar(.1))
nslow2 = 0;
/* test for convergence. */
if (delta <= parameters.xtol * xnorm || fnorm == 0.)
return RelativeErrorTooSmall;
/* tests for termination and stringent tolerances. */
if (nfev >= parameters.maxfev)
return TooManyFunctionEvaluation;
if (Scalar(.1) * std::max(Scalar(.1) * delta, pnorm) <= epsilon<Scalar>() * xnorm)
return TolTooSmall;
if (nslow2 == 5)
return NotMakingProgressJacobian;
if (nslow1 == 10)
return NotMakingProgressIterations;
/* criterion for recalculating jacobian. */
if (ncfail == 2)
break; // leave inner loop and go for the next outer loop iteration
/* calculate the rank one modification to the jacobian */
/* and update qtf if necessary. */
for (j = 0; j < n; ++j) {
sum = wa4.dot(fjac.col(j));
wa2[j] = (sum - wa3[j]) / pnorm;
wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm);
if (ratio >= Scalar(1e-4))
qtf[j] = sum;
}
/* compute the qr factorization of the updated jacobian. */
ei_r1updt<Scalar>(n, n, R.data(), R.size(), wa1.data(), wa2.data(), wa3.data(), &sing);
ei_r1mpyq<Scalar>(n, n, fjac.data(), fjac.rows(), wa2.data(), wa3.data());
ei_r1mpyq<Scalar>(1, n, qtf.data(), 1, wa2.data(), wa3.data());
/* end of the inner loop. */
jeval = false;
}
/* end of the outer loop. */
return Running;
}
template<typename FunctorType, typename Scalar>
typename HybridNonLinearSolver<FunctorType,Scalar>::Status
HybridNonLinearSolver<FunctorType,Scalar>::solve(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
Status status = solveInit(x, mode);
while (status==Running)
status = solveOneStep(x, mode);
return status;
}
template<typename FunctorType, typename Scalar>
typename HybridNonLinearSolver<FunctorType,Scalar>::Status
HybridNonLinearSolver<FunctorType,Scalar>::hybrd1(
Matrix< Scalar, Dynamic, 1 > &x,
const Scalar tol
)
{
n = x.size();
/* check the input parameters for errors. */
if (n <= 0 || tol < 0.)
return ImproperInputParameters;
resetParameters();
parameters.maxfev = 200*(n+1);
parameters.xtol = tol;
diag.setConstant(n, 1.);
return solveNumericalDiff(
x,
2
);
}
template<typename FunctorType, typename Scalar>
typename HybridNonLinearSolver<FunctorType,Scalar>::Status
HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
n = x.size();
if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n);
qtf.resize(n);
R.resize( (n*(n+1))/2);
fjac.resize(n, n);
fvec.resize(n);
if (mode != 2)
diag.resize(n);
assert( (mode!=2 || diag.size()==n) || "When using mode==2, the caller must provide a valid 'diag'");
/* Function Body */
nfev = 0;
njev = 0;
/* check the input parameters for errors. */
if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. )
return ImproperInputParameters;
if (mode == 2)
for (int j = 0; j < n; ++j)
if (diag[j] <= 0.)
return ImproperInputParameters;
/* evaluate the function at the starting point */
/* and calculate its norm. */
nfev = 1;
if ( functor(x, fvec) < 0)
return UserAksed;
fnorm = fvec.stableNorm();
/* initialize iteration counter and monitors. */
iter = 1;
ncsuc = 0;
ncfail = 0;
nslow1 = 0;
nslow2 = 0;
return Running;
}
template<typename FunctorType, typename Scalar>
typename HybridNonLinearSolver<FunctorType,Scalar>::Status
HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
int i, j, l, iwa[1];
jeval = true;
if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
/* calculate the jacobian matrix. */
if (ei_fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals, parameters.epsfcn) <0)
return UserAksed;
nfev += std::min(parameters.nb_of_subdiagonals+parameters.nb_of_superdiagonals+ 1, n);
/* compute the qr factorization of the jacobian. */
ei_qrfac<Scalar>(n, n, fjac.data(), fjac.rows(), false, iwa, wa1.data(), wa2.data());
/* on the first iteration and if mode is 1, scale according */
/* to the norms of the columns of the initial jacobian. */
if (iter == 1) {
if (mode != 2)
for (j = 0; j < n; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.)
diag[j] = 1.;
}
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
wa3 = diag.cwise() * x;
xnorm = wa3.stableNorm();
delta = parameters.factor * xnorm;
if (delta == 0.)
delta = parameters.factor;
}
/* form (q transpose)*fvec and store in qtf. */
qtf = fvec;
for (j = 0; j < n; ++j)
if (fjac(j,j) != 0.) {
sum = 0.;
for (i = j; i < n; ++i)
sum += fjac(i,j) * qtf[i];
temp = -sum / fjac(j,j);
for (i = j; i < n; ++i)
qtf[i] += fjac(i,j) * temp;
}
/* copy the triangular factor of the qr factorization into r. */
sing = false;
for (j = 0; j < n; ++j) {
l = j;
if (j)
for (i = 0; i < j; ++i) {
R[l] = fjac(i,j);
l = l + n - i -1;
}
R[l] = wa1[j];
if (wa1[j] == 0.)
sing = true;
}
/* accumulate the orthogonal factor in fjac. */
ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data());
/* rescale if necessary. */
/* Computing MAX */
if (mode != 2)
diag = diag.cwise().max(wa2);
/* beginning of the inner loop. */
while (true) {
/* determine the direction p. */
ei_dogleg<Scalar>(R, diag, qtf, delta, wa1);
/* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1;
wa2 = x + wa1;
wa3 = diag.cwise() * wa1;
pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */
if (iter == 1)
delta = std::min(delta,pnorm);
/* evaluate the function at x + p and calculate its norm. */
if ( functor(wa2, wa4) < 0)
return UserAksed;
++nfev;
fnorm1 = wa4.stableNorm();
/* compute the scaled actual reduction. */
actred = -1.;
if (fnorm1 < fnorm) /* Computing 2nd power */
actred = 1. - ei_abs2(fnorm1 / fnorm);
/* compute the scaled predicted reduction. */
l = 0;
for (i = 0; i < n; ++i) {
sum = 0.;
for (j = i; j < n; ++j) {
sum += R[l] * wa1[j];
++l;
}
wa3[i] = qtf[i] + sum;
}
temp = wa3.stableNorm();
prered = 0.;
if (temp < fnorm) /* Computing 2nd power */
prered = 1. - ei_abs2(temp / fnorm);
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered > 0.)
ratio = actred / prered;
/* update the step bound. */
if (ratio < Scalar(.1)) {
ncsuc = 0;
++ncfail;
delta = Scalar(.5) * delta;
} else {
ncfail = 0;
++ncsuc;
if (ratio >= Scalar(.5) || ncsuc > 1) /* Computing MAX */
delta = std::max(delta, pnorm / Scalar(.5));
if (ei_abs(ratio - 1.) <= Scalar(.1)) {
delta = pnorm / Scalar(.5);
}
}
/* test for successful iteration. */
if (ratio >= Scalar(1e-4)) {
/* successful iteration. update x, fvec, and their norms. */
x = wa2;
wa2 = diag.cwise() * x;
fvec = wa4;
xnorm = wa2.stableNorm();
fnorm = fnorm1;
++iter;
}
/* determine the progress of the iteration. */
++nslow1;
if (actred >= Scalar(.001))
nslow1 = 0;
if (jeval)
++nslow2;
if (actred >= Scalar(.1))
nslow2 = 0;
/* test for convergence. */
if (delta <= parameters.xtol * xnorm || fnorm == 0.)
return RelativeErrorTooSmall;
/* tests for termination and stringent tolerances. */
if (nfev >= parameters.maxfev)
return TooManyFunctionEvaluation;
if (Scalar(.1) * std::max(Scalar(.1) * delta, pnorm) <= epsilon<Scalar>() * xnorm)
return TolTooSmall;
if (nslow2 == 5)
return NotMakingProgressJacobian;
if (nslow1 == 10)
return NotMakingProgressIterations;
/* criterion for recalculating jacobian approximation */
/* by forward differences. */
if (ncfail == 2)
break; // leave inner loop and go for the next outer loop iteration
/* calculate the rank one modification to the jacobian */
/* and update qtf if necessary. */
for (j = 0; j < n; ++j) {
sum = wa4.dot(fjac.col(j));
wa2[j] = (sum - wa3[j]) / pnorm;
wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm);
if (ratio >= Scalar(1e-4))
qtf[j] = sum;
}
/* compute the qr factorization of the updated jacobian. */
ei_r1updt<Scalar>(n, n, R.data(), R.size(), wa1.data(), wa2.data(), wa3.data(), &sing);
ei_r1mpyq<Scalar>(n, n, fjac.data(), fjac.rows(), wa2.data(), wa3.data());
ei_r1mpyq<Scalar>(1, n, qtf.data(), 1, wa2.data(), wa3.data());
/* end of the inner loop. */
jeval = false;
}
/* end of the outer loop. */
return Running;
}
template<typename FunctorType, typename Scalar>
typename HybridNonLinearSolver<FunctorType,Scalar>::Status
HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
Status status = solveNumericalDiffInit(x, mode);
while (status==Running)
status = solveNumericalDiffOneStep(x, mode);
return status;
}

723
unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h Normal file
View File

@@ -0,0 +1,723 @@
/**
* \brief Performs non linear optimization over a non-linear function,
* using a variant of the Levenberg Marquardt algorithm.
*
* Check wikipedia for more information.
* http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
*/
template<typename FunctorType, typename Scalar=double>
class LevenbergMarquardt
{
public:
LevenbergMarquardt(FunctorType &_functor)
: functor(_functor) { nfev = njev = iter = 0; fnorm=gnorm = 0.; }
enum Status {
Running = -1,
ImproperInputParameters = 0,
RelativeReductionTooSmall = 1,
RelativeErrorTooSmall = 2,
RelativeErrorAndReductionTooSmall = 3,
CosinusTooSmall = 4,
TooManyFunctionEvaluation = 5,
FtolTooSmall = 6,
XtolTooSmall = 7,
GtolTooSmall = 8,
UserAsked = 9
};
struct Parameters {
Parameters()
: factor(Scalar(100.))
, maxfev(400)
, ftol(ei_sqrt(epsilon<Scalar>()))
, xtol(ei_sqrt(epsilon<Scalar>()))
, gtol(Scalar(0.))
, epsfcn(Scalar(0.)) {}
Scalar factor;
int maxfev; // maximum number of function evaluation
Scalar ftol;
Scalar xtol;
Scalar gtol;
Scalar epsfcn;
};
Status lmder1(
Matrix< Scalar, Dynamic, 1 > &x,
const Scalar tol = ei_sqrt(epsilon<Scalar>())
);
Status minimize(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
Status minimizeInit(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
Status minimizeOneStep(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
static Status lmdif1(
FunctorType &functor,
Matrix< Scalar, Dynamic, 1 > &x,
int *nfev,
const Scalar tol = ei_sqrt(epsilon<Scalar>())
);
Status lmstr1(
Matrix< Scalar, Dynamic, 1 > &x,
const Scalar tol = ei_sqrt(epsilon<Scalar>())
);
Status minimizeOptimumStorage(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
Status minimizeOptimumStorageInit(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
Status minimizeOptimumStorageOneStep(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode=1
);
void resetParameters(void) { parameters = Parameters(); }
Parameters parameters;
Matrix< Scalar, Dynamic, 1 > fvec;
Matrix< Scalar, Dynamic, Dynamic > fjac;
VectorXi ipvt;
Matrix< Scalar, Dynamic, 1 > qtf;
Matrix< Scalar, Dynamic, 1 > diag;
int nfev;
int njev;
int iter;
Scalar fnorm, gnorm;
private:
FunctorType &functor;
int n;
int m;
Matrix< Scalar, Dynamic, 1 > wa1, wa2, wa3, wa4;
Scalar par, sum;
Scalar temp, temp1, temp2;
Scalar delta;
Scalar ratio;
Scalar pnorm, xnorm, fnorm1, actred, dirder, prered;
};
template<typename FunctorType, typename Scalar>
typename LevenbergMarquardt<FunctorType,Scalar>::Status
LevenbergMarquardt<FunctorType,Scalar>::lmder1(
Matrix< Scalar, Dynamic, 1 > &x,
const Scalar tol
)
{
n = x.size();
m = functor.values();
/* check the input parameters for errors. */
if (n <= 0 || m < n || tol < 0.)
return ImproperInputParameters;
resetParameters();
parameters.ftol = tol;
parameters.xtol = tol;
parameters.maxfev = 100*(n+1);
return minimize(x);
}
template<typename FunctorType, typename Scalar>
typename LevenbergMarquardt<FunctorType,Scalar>::Status
LevenbergMarquardt<FunctorType,Scalar>::minimize(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
Status status = minimizeInit(x, mode);
while (status==Running)
status = minimizeOneStep(x, mode);
return status;
}
template<typename FunctorType, typename Scalar>
typename LevenbergMarquardt<FunctorType,Scalar>::Status
LevenbergMarquardt<FunctorType,Scalar>::minimizeInit(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
n = x.size();
m = functor.values();
wa1.resize(n); wa2.resize(n); wa3.resize(n);
wa4.resize(m);
fvec.resize(m);
ipvt.resize(n);
fjac.resize(m, n);
if (mode != 2)
diag.resize(n);
assert( (mode!=2 || diag.size()==n) || "When using mode==2, the caller must provide a valid 'diag'");
qtf.resize(n);
/* Function Body */
nfev = 0;
njev = 0;
/* check the input parameters for errors. */
if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
return ImproperInputParameters;
if (mode == 2)
for (int j = 0; j < n; ++j)
if (diag[j] <= 0.)
return ImproperInputParameters;
/* evaluate the function at the starting point */
/* and calculate its norm. */
nfev = 1;
if ( functor(x, fvec) < 0)
return UserAsked;
fnorm = fvec.stableNorm();
/* initialize levenberg-marquardt parameter and iteration counter. */
par = 0.;
iter = 1;
return Running;
}
template<typename FunctorType, typename Scalar>
typename LevenbergMarquardt<FunctorType,Scalar>::Status
LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
int i, j, l;
/* calculate the jacobian matrix. */
int df_ret = functor.df(x, fjac);
if (df_ret<0)
return UserAsked;
if (df_ret>0)
// numerical diff, we evaluated the function df_ret times
nfev += df_ret;
else njev++;
/* compute the qr factorization of the jacobian. */
ei_qrfac<Scalar>(m, n, fjac.data(), fjac.rows(), true, ipvt.data(), wa1.data(), wa2.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. */
if (iter == 1) {
if (mode != 2)
for (j = 0; j < n; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.)
diag[j] = 1.;
}
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
wa3 = diag.cwise() * x;
xnorm = wa3.stableNorm();
delta = parameters.factor * xnorm;
if (delta == 0.)
delta = parameters.factor;
}
/* form (q transpose)*fvec and store the first n components in */
/* qtf. */
wa4 = fvec;
for (j = 0; j < n; ++j) {
if (fjac(j,j) != 0.) {
sum = 0.;
for (i = j; i < m; ++i)
sum += fjac(i,j) * wa4[i];
temp = -sum / fjac(j,j);
for (i = j; i < m; ++i)
wa4[i] += fjac(i,j) * temp;
}
fjac(j,j) = wa1[j];
qtf[j] = wa4[j];
}
/* compute the norm of the scaled gradient. */
gnorm = 0.;
if (fnorm != 0.)
for (j = 0; j < n; ++j) {
l = ipvt[j];
if (wa2[l] != 0.) {
sum = 0.;
for (i = 0; i <= j; ++i)
sum += fjac(i,j) * (qtf[i] / fnorm);
/* Computing MAX */
gnorm = std::max(gnorm, ei_abs(sum / wa2[l]));
}
}
/* test for convergence of the gradient norm. */
if (gnorm <= parameters.gtol)
return CosinusTooSmall;
/* rescale if necessary. */
if (mode != 2) /* Computing MAX */
diag = diag.cwise().max(wa2);
/* beginning of the inner loop. */
do {
/* determine the levenberg-marquardt parameter. */
ei_lmpar<Scalar>(fjac, ipvt, diag, qtf, delta, par, wa1, wa2);
/* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1;
wa2 = x + wa1;
wa3 = diag.cwise() * wa1;
pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */
if (iter == 1)
delta = std::min(delta,pnorm);
/* evaluate the function at x + p and calculate its norm. */
if ( functor(wa2, wa4) < 0)
return UserAsked;
++nfev;
fnorm1 = wa4.stableNorm();
/* compute the scaled actual reduction. */
actred = -1.;
if (Scalar(.1) * fnorm1 < fnorm) /* Computing 2nd power */
actred = 1. - ei_abs2(fnorm1 / fnorm);
/* compute the scaled predicted reduction and */
/* the scaled directional derivative. */
wa3.fill(0.);
for (j = 0; j < n; ++j) {
l = ipvt[j];
temp = wa1[l];
for (i = 0; i <= j; ++i)
wa3[i] += fjac(i,j) * temp;
}
temp1 = ei_abs2(wa3.stableNorm() / fnorm);
temp2 = ei_abs2(ei_sqrt(par) * pnorm / fnorm);
/* Computing 2nd power */
prered = temp1 + temp2 / Scalar(.5);
dirder = -(temp1 + temp2);
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered != 0.)
ratio = actred / prered;
/* update the step bound. */
if (ratio <= Scalar(.25)) {
if (actred >= 0.)
temp = Scalar(.5);
if (actred < 0.)
temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
temp = Scalar(.1);
/* Computing MIN */
delta = temp * std::min(delta, pnorm / Scalar(.1));
par /= temp;
} else if (!(par != 0. && ratio < Scalar(.75))) {
delta = pnorm / Scalar(.5);
par = Scalar(.5) * par;
}
/* test for successful iteration. */
if (ratio >= Scalar(1e-4)) {
/* successful iteration. update x, fvec, and their norms. */
x = wa2;
wa2 = diag.cwise() * x;
fvec = wa4;
xnorm = wa2.stableNorm();
fnorm = fnorm1;
++iter;
}
/* tests for convergence. */
if (ei_abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
return RelativeErrorAndReductionTooSmall;
if (ei_abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
return RelativeReductionTooSmall;
if (delta <= parameters.xtol * xnorm)
return RelativeErrorTooSmall;
/* tests for termination and stringent tolerances. */
if (nfev >= parameters.maxfev)
return TooManyFunctionEvaluation;
if (ei_abs(actred) <= epsilon<Scalar>() && prered <= epsilon<Scalar>() && Scalar(.5) * ratio <= 1.)
return FtolTooSmall;
if (delta <= epsilon<Scalar>() * xnorm)
return XtolTooSmall;
if (gnorm <= epsilon<Scalar>())
return GtolTooSmall;
/* end of the inner loop. repeat if iteration unsuccessful. */
} while (ratio < Scalar(1e-4));
/* end of the outer loop. */
return Running;
}
template<typename FunctorType, typename Scalar>
typename LevenbergMarquardt<FunctorType,Scalar>::Status
LevenbergMarquardt<FunctorType,Scalar>::lmstr1(
Matrix< Scalar, Dynamic, 1 > &x,
const Scalar tol
)
{
n = x.size();
m = functor.values();
Matrix< Scalar, Dynamic, Dynamic > fjac(m, n);
VectorXi ipvt;
/* check the input parameters for errors. */
if (n <= 0 || m < n || tol < 0.)
return ImproperInputParameters;
resetParameters();
parameters.ftol = tol;
parameters.xtol = tol;
parameters.maxfev = 100*(n+1);
return minimizeOptimumStorage(x);
}
template<typename FunctorType, typename Scalar>
typename LevenbergMarquardt<FunctorType,Scalar>::Status
LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageInit(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
n = x.size();
m = functor.values();
wa1.resize(n); wa2.resize(n); wa3.resize(n);
wa4.resize(m);
fvec.resize(m);
ipvt.resize(n);
fjac.resize(m, n);
if (mode != 2)
diag.resize(n);
assert( (mode!=2 || diag.size()==n) || "When using mode==2, the caller must provide a valid 'diag'");
qtf.resize(n);
/* Function Body */
nfev = 0;
njev = 0;
/* check the input parameters for errors. */
if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
return ImproperInputParameters;
if (mode == 2)
for (int j = 0; j < n; ++j)
if (diag[j] <= 0.)
return ImproperInputParameters;
/* evaluate the function at the starting point */
/* and calculate its norm. */
nfev = 1;
if ( functor(x, fvec) < 0)
return UserAsked;
fnorm = fvec.stableNorm();
/* initialize levenberg-marquardt parameter and iteration counter. */
par = 0.;
iter = 1;
return Running;
}
template<typename FunctorType, typename Scalar>
typename LevenbergMarquardt<FunctorType,Scalar>::Status
LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
int i, j, l;
bool sing;
/* 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. */
qtf.fill(0.);
fjac.fill(0.);
int rownb = 2;
for (i = 0; i < m; ++i) {
if (functor.df(x, wa3, rownb) < 0) return UserAsked;
temp = fvec[i];
ei_rwupdt<Scalar>(n, fjac.data(), fjac.rows(), wa3.data(), qtf.data(), &temp, wa1.data(), wa2.data());
++rownb;
}
++njev;
/* if the jacobian is rank deficient, call qrfac to */
/* reorder its columns and update the components of qtf. */
sing = false;
for (j = 0; j < n; ++j) {
if (fjac(j,j) == 0.) {
sing = true;
}
ipvt[j] = j;
wa2[j] = fjac.col(j).start(j).stableNorm();
}
if (sing) {
ipvt.cwise()+=1;
ei_qrfac<Scalar>(n, n, fjac.data(), fjac.rows(), true, ipvt.data(), wa1.data(), wa2.data());
ipvt.cwise()-=1; // qrfac() creates ipvt with fortran convetion (1->n), convert it to c (0->n-1)
for (j = 0; j < n; ++j) {
if (fjac(j,j) != 0.) {
sum = 0.;
for (i = j; i < n; ++i)
sum += fjac(i,j) * qtf[i];
temp = -sum / fjac(j,j);
for (i = j; i < n; ++i)
qtf[i] += fjac(i,j) * temp;
}
fjac(j,j) = wa1[j];
}
}
/* on the first iteration and if mode is 1, scale according */
/* to the norms of the columns of the initial jacobian. */
if (iter == 1) {
if (mode != 2)
for (j = 0; j < n; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.)
diag[j] = 1.;
}
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
wa3 = diag.cwise() * x;
xnorm = wa3.stableNorm();
delta = parameters.factor * xnorm;
if (delta == 0.)
delta = parameters.factor;
}
/* compute the norm of the scaled gradient. */
gnorm = 0.;
if (fnorm != 0.)
for (j = 0; j < n; ++j) {
l = ipvt[j];
if (wa2[l] != 0.) {
sum = 0.;
for (i = 0; i <= j; ++i)
sum += fjac(i,j) * (qtf[i] / fnorm);
/* Computing MAX */
gnorm = std::max(gnorm, ei_abs(sum / wa2[l]));
}
}
/* test for convergence of the gradient norm. */
if (gnorm <= parameters.gtol)
return CosinusTooSmall;
/* rescale if necessary. */
if (mode != 2) /* Computing MAX */
diag = diag.cwise().max(wa2);
/* beginning of the inner loop. */
do {
/* determine the levenberg-marquardt parameter. */
ei_lmpar<Scalar>(fjac, ipvt, diag, qtf, delta, par, wa1, wa2);
/* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1;
wa2 = x + wa1;
wa3 = diag.cwise() * wa1;
pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */
if (iter == 1)
delta = std::min(delta,pnorm);
/* evaluate the function at x + p and calculate its norm. */
if ( functor(wa2, wa4) < 0)
return UserAsked;
++nfev;
fnorm1 = wa4.stableNorm();
/* compute the scaled actual reduction. */
actred = -1.;
if (Scalar(.1) * fnorm1 < fnorm) /* Computing 2nd power */
actred = 1. - ei_abs2(fnorm1 / fnorm);
/* compute the scaled predicted reduction and */
/* the scaled directional derivative. */
wa3.fill(0.);
for (j = 0; j < n; ++j) {
l = ipvt[j];
temp = wa1[l];
for (i = 0; i <= j; ++i)
wa3[i] += fjac(i,j) * temp;
}
temp1 = ei_abs2(wa3.stableNorm() / fnorm);
temp2 = ei_abs2(ei_sqrt(par) * pnorm / fnorm);
/* Computing 2nd power */
prered = temp1 + temp2 / Scalar(.5);
dirder = -(temp1 + temp2);
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered != 0.)
ratio = actred / prered;
/* update the step bound. */
if (ratio <= Scalar(.25)) {
if (actred >= 0.)
temp = Scalar(.5);
if (actred < 0.)
temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
temp = Scalar(.1);
/* Computing MIN */
delta = temp * std::min(delta, pnorm / Scalar(.1));
par /= temp;
} else if (!(par != 0. && ratio < Scalar(.75))) {
delta = pnorm / Scalar(.5);
par = Scalar(.5) * par;
}
/* test for successful iteration. */
if (ratio >= Scalar(1e-4)) {
/* successful iteration. update x, fvec, and their norms. */
x = wa2;
wa2 = diag.cwise() * x;
fvec = wa4;
xnorm = wa2.stableNorm();
fnorm = fnorm1;
++iter;
}
/* tests for convergence. */
if (ei_abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
return RelativeErrorAndReductionTooSmall;
if (ei_abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
return RelativeReductionTooSmall;
if (delta <= parameters.xtol * xnorm)
return RelativeErrorTooSmall;
/* tests for termination and stringent tolerances. */
if (nfev >= parameters.maxfev)
return TooManyFunctionEvaluation;
if (ei_abs(actred) <= epsilon<Scalar>() && prered <= epsilon<Scalar>() && Scalar(.5) * ratio <= 1.)
return FtolTooSmall;
if (delta <= epsilon<Scalar>() * xnorm)
return XtolTooSmall;
if (gnorm <= epsilon<Scalar>())
return GtolTooSmall;
/* end of the inner loop. repeat if iteration unsuccessful. */
} while (ratio < Scalar(1e-4));
/* end of the outer loop. */
return Running;
}
template<typename FunctorType, typename Scalar>
typename LevenbergMarquardt<FunctorType,Scalar>::Status
LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorage(
Matrix< Scalar, Dynamic, 1 > &x,
const int mode
)
{
Status status = minimizeOptimumStorageInit(x, mode);
while (status==Running)
status = minimizeOptimumStorageOneStep(x, mode);
return status;
}
template<typename FunctorType, typename Scalar>
typename LevenbergMarquardt<FunctorType,Scalar>::Status
LevenbergMarquardt<FunctorType,Scalar>::lmdif1(
FunctorType &functor,
Matrix< Scalar, Dynamic, 1 > &x,
int *nfev,
const Scalar tol
)
{
int n = x.size();
int m = functor.values();
/* check the input parameters for errors. */
if (n <= 0 || m < n || tol < 0.)
return ImproperInputParameters;
NumericalDiff<FunctorType> numDiff(functor);
// embedded LevenbergMarquardt
LevenbergMarquardt<NumericalDiff<FunctorType> > lm(numDiff);
lm.parameters.ftol = tol;
lm.parameters.xtol = tol;
lm.parameters.maxfev = 200*(n+1);
Status info = Status(lm.minimize(x));
if (nfev)
* nfev = lm.nfev;
return info;
}

55
unsupported/Eigen/src/NonLinearOptimization/chkder.h Normal file
View File

@@ -0,0 +1,55 @@
#define chkder_log10e 0.43429448190325182765
#define chkder_factor 100.
template<typename Scalar>
void ei_chkder(
Matrix< Scalar, Dynamic, 1 > &x,
Matrix< Scalar, Dynamic, 1 > &fvec,
Matrix< Scalar, Dynamic, Dynamic > &fjac,
Matrix< Scalar, Dynamic, 1 > &xp,
Matrix< Scalar, Dynamic, 1 > &fvecp,
int mode,
Matrix< Scalar, Dynamic, 1 > &err
)
{
const Scalar eps = ei_sqrt(epsilon<Scalar>());
const Scalar epsf = chkder_factor * epsilon<Scalar>();
const Scalar epslog = chkder_log10e * ei_log(eps);
Scalar temp;
int i,j;
const int m = fvec.size(), n = x.size();
if (mode != 2) {
xp.resize(n);
/* mode = 1. */
for (j = 0; j < n; ++j) {
temp = eps * ei_abs(x[j]);
if (temp == 0.)
temp = eps;
xp[j] = x[j] + temp;
}
}
else {
/* mode = 2. */
err.setZero(m);
for (j = 0; j < n; ++j) {
temp = ei_abs(x[j]);
if (temp == 0.)
temp = 1.;
err += temp * fjac.col(j);
}
for (i = 0; i < m; ++i) {
temp = 1.;
if (fvec[i] != 0. && fvecp[i] != 0. && ei_abs(fvecp[i] - fvec[i]) >= epsf * ei_abs(fvec[i]))
temp = eps * ei_abs((fvecp[i] - fvec[i]) / eps - err[i]) / (ei_abs(fvec[i]) + ei_abs(fvecp[i]));
err[i] = 1.;
if (temp > epsilon<Scalar>() && temp < eps)
err[i] = (chkder_log10e * ei_log(temp) - epslog) / epslog;
if (temp >= eps)
err[i] = 0.;
}
}
} /* chkder_ */

74
unsupported/Eigen/src/NonLinearOptimization/covar.h Normal file
View File

@@ -0,0 +1,74 @@
template <typename Scalar>
void ei_covar(
Matrix< Scalar, Dynamic, Dynamic > &r,
const VectorXi &ipvt,
Scalar tol = ei_sqrt(epsilon<Scalar>()) )
{
/* Local variables */
int i, j, k, l, ii, jj;
int sing;
Scalar temp;
/* Function Body */
const int n = r.cols();
const Scalar tolr = tol * ei_abs(r[0]);
Matrix< Scalar, Dynamic, 1 > wa(n);
assert(ipvt.size()==n);
/* form the inverse of r in the full upper triangle of r. */
l = -1;
for (k = 0; k < n; ++k)
if (ei_abs(r(k,k)) > tolr) {
r(k,k) = 1. / r(k,k);
for (j = 0; j <= k-1; ++j) {
temp = r(k,k) * r(j,k);
r(j,k) = 0.;
for (i = 0; i <= j; ++i)
r(i,k) -= temp * r(i,j);
}
l = k;
}
/* form the full upper triangle of the inverse of (r transpose)*r */
/* in the full upper triangle of r. */
for (k = 0; k <= l; ++k) {
for (j = 0; j <= k-1; ++j) {
temp = r(j,k);
for (i = 0; i <= j; ++i)
r(i,j) += temp * r(i,k);
}
temp = r(k,k);
for (i = 0; i <= k; ++i)
r(i,k) = temp * r(i,k);
}
/* form the full lower triangle of the covariance matrix */
/* in the strict lower triangle of r and in wa. */
for (j = 0; j < n; ++j) {
jj = ipvt[j];
sing = j > l;
for (i = 0; i <= j; ++i) {
if (sing)
r(i,j) = 0.;
ii = ipvt[i];
if (ii > jj)
r(ii,jj) = r(i,j);
if (ii < jj)
r(jj,ii) = r(i,j);
}
wa[jj] = r(j,j);
}
/* symmetrize the covariance matrix in r. */
for (j = 0; j < n; ++j) {
for (i = 0; i <= j; ++i)
r(i,j) = r(j,i);
r(j,j) = wa[j];
}
}

124
unsupported/Eigen/src/NonLinearOptimization/dogleg.h Normal file
View File

@@ -0,0 +1,124 @@
template <typename Scalar>
void ei_dogleg(
Matrix< Scalar, Dynamic, 1 > &r,
const Matrix< Scalar, Dynamic, 1 > &diag,
const Matrix< Scalar, Dynamic, 1 > &qtb,
Scalar delta,
Matrix< Scalar, Dynamic, 1 > &x)
{
/* Local variables */
int i, j, k, l, jj;
Scalar sum, temp, alpha, bnorm;
Scalar gnorm, qnorm;
Scalar sgnorm;
/* Function Body */
const Scalar epsmch = epsilon<Scalar>();
const int n = diag.size();
Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n);
assert(n==qtb.size());
assert(n==x.size());
/* first, calculate the gauss-newton direction. */
jj = n * (n + 1) / 2;
for (k = 0; k < n; ++k) {
j = n - k - 1;
jj -= k+1;
l = jj + 1;
sum = 0.;
for (i = j+1; i < n; ++i) {
sum += r[l] * x[i];
++l;
}
temp = r[jj];
if (temp == 0.) {
l = j;
for (i = 0; i <= j; ++i) {
/* Computing MAX */
temp = std::max(temp,ei_abs(r[l]));
l = l + n - i;
}
temp = epsmch * temp;
if (temp == 0.)
temp = epsmch;
}
x[j] = (qtb[j] - sum) / temp;
}
/* test whether the gauss-newton direction is acceptable. */
wa1.fill(0.);
wa2 = diag.cwise() * x;
qnorm = wa2.stableNorm();
if (qnorm <= delta)
return;
/* the gauss-newton direction is not acceptable. */
/* next, calculate the scaled gradient direction. */
l = 0;
for (j = 0; j < n; ++j) {
temp = qtb[j];
for (i = j; i < n; ++i) {
wa1[i] += r[l] * temp;
++l;
}
wa1[j] /= diag[j];
}
/* calculate the norm of the scaled gradient and test for */
/* the special case in which the scaled gradient is zero. */
gnorm = wa1.stableNorm();
sgnorm = 0.;
alpha = delta / qnorm;
if (gnorm == 0.)
goto algo_end;
/* calculate the point along the scaled gradient */
/* at which the quadratic is minimized. */
wa1.cwise() /= diag*gnorm;
l = 0;
for (j = 0; j < n; ++j) {
sum = 0.;
for (i = j; i < n; ++i) {
sum += r[l] * wa1[i];
++l;
/* L100: */
}
wa2[j] = sum;
/* L110: */
}
temp = wa2.stableNorm();
sgnorm = gnorm / temp / temp;
/* test whether the scaled gradient direction is acceptable. */
alpha = 0.;
if (sgnorm >= delta)
goto algo_end;
/* the scaled gradient direction is not acceptable. */
/* finally, calculate the point along the dogleg */
/* at which the quadratic is minimized. */
bnorm = qtb.stableNorm();
temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
/* Computing 2nd power */
temp = temp - delta / qnorm * ei_abs2(sgnorm / delta) + ei_sqrt(ei_abs2(temp - delta / qnorm) + (1.-ei_abs2(delta / qnorm)) * (1.-ei_abs2(sgnorm / delta)));
/* Computing 2nd power */
alpha = delta / qnorm * (1. - ei_abs2(sgnorm / delta)) / temp;
algo_end:
/* form appropriate convex combination of the gauss-newton */
/* direction and the scaled gradient direction. */
temp = (1.-alpha) * std::min(sgnorm,delta);
x = temp * wa1 + alpha * x;
return;
}

70
unsupported/Eigen/src/NonLinearOptimization/fdjac1.h Normal file
View File

@@ -0,0 +1,70 @@
template<typename FunctorType, typename Scalar>
int ei_fdjac1(
const FunctorType &Functor,
Matrix< Scalar, Dynamic, 1 > &x,
Matrix< Scalar, Dynamic, 1 > &fvec,
Matrix< Scalar, Dynamic, Dynamic > &fjac,
int ml, int mu,
Scalar epsfcn)
{
/* Local variables */
Scalar h;
int i, j, k;
Scalar eps, temp;
int msum;
int iflag = 0;
/* Function Body */
const Scalar epsmch = epsilon<Scalar>();
const int n = x.size();
assert(fvec.size()==n);
Matrix< Scalar, Dynamic, 1 > wa1(n);
Matrix< Scalar, Dynamic, 1 > wa2(n);
eps = ei_sqrt((std::max(epsfcn,epsmch)));
msum = ml + mu + 1;
if (msum >= n) {
/* computation of dense approximate jacobian. */
for (j = 0; j < n; ++j) {
temp = x[j];
h = eps * ei_abs(temp);
if (h == 0.)
h = eps;
x[j] = temp + h;
iflag = Functor(x, wa1);
if (iflag < 0)
return iflag;
x[j] = temp;
fjac.col(j) = (wa1-fvec)/h;
}
}else {
/* computation of banded approximate jacobian. */
for (k = 0; k < msum; ++k) {
for (j = k; msum< 0 ? j > n: j < n; j += msum) {
wa2[j] = x[j];
h = eps * ei_abs(wa2[j]);
if (h == 0.) h = eps;
x[j] = wa2[j] + h;
}
iflag = Functor(x, wa1);
if (iflag < 0) {
return iflag;
}
for (j = k; msum< 0 ? j > n: j < n; j += msum) {
x[j] = wa2[j];
h = eps * ei_abs(wa2[j]);
if (h == 0.) h = eps;
for (i = 0; i < n; ++i) {
fjac(i,j) = 0.;
if (i >= j - mu && i <= j + ml) {
fjac(i,j) = (wa1[i] - fvec[i]) / h;
}
}
}
}
}
return iflag;
} /* fdjac1_ */

179
unsupported/Eigen/src/NonLinearOptimization/lmpar.h Normal file
View File

@@ -0,0 +1,179 @@
template <typename Scalar>
void ei_lmpar(
Matrix< Scalar, Dynamic, Dynamic > &r,
VectorXi &ipvt, // TODO : const once ipvt mess fixed
const Matrix< Scalar, Dynamic, 1 > &diag,
const Matrix< Scalar, Dynamic, 1 > &qtb,
Scalar delta,
Scalar &par,
Matrix< Scalar, Dynamic, 1 > &x,
Matrix< Scalar, Dynamic, 1 > &sdiag)
{
/* Local variables */
int i, j, k, l;
Scalar fp;
Scalar sum, parc, parl;
int iter;
Scalar temp, paru;
int nsing;
Scalar gnorm;
Scalar dxnorm;
/* Function Body */
const Scalar dwarf = std::numeric_limits<Scalar>::min();
const int n = r.cols();
assert(n==diag.size());
assert(n==qtb.size());
assert(n==x.size());
Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n);
/* compute and store in x the gauss-newton direction. if the */
/* jacobian is rank-deficient, obtain a least squares solution. */
nsing = n-1;
for (j = 0; j < n; ++j) {
wa1[j] = qtb[j];
if (r(j,j) == 0. && nsing == n-1)
nsing = j - 1;
if (nsing < n-1)
wa1[j] = 0.;
}
for (k = 0; k <= nsing; ++k) {
j = nsing - k;
wa1[j] /= r(j,j);
temp = wa1[j];
for (i = 0; i < j ; ++i)
wa1[i] -= r(i,j) * temp;
}
for (j = 0; j < n; ++j) {
l = ipvt[j];
x[l] = wa1[j];
}
/* initialize the iteration counter. */
/* evaluate the function at the origin, and test */
/* for acceptance of the gauss-newton direction. */
iter = 0;
wa2 = diag.cwise() * x;
dxnorm = wa2.blueNorm();
fp = dxnorm - delta;
if (fp <= Scalar(0.1) * delta) {
par = 0;
return;
}
/* if the jacobian is not rank deficient, the newton */
/* step provides a lower bound, parl, for the zero of */
/* the function. otherwise set this bound to zero. */
parl = 0.;
if (nsing >= n-1) {
for (j = 0; j < n; ++j) {
l = ipvt[j];
wa1[j] = diag[l] * (wa2[l] / dxnorm);
}
for (j = 0; j < n; ++j) {
sum = 0.;
for (i = 0; i < j; ++i)
sum += r(i,j) * wa1[i];
wa1[j] = (wa1[j] - sum) / r(j,j);
}
temp = wa1.blueNorm();
parl = fp / delta / temp / temp;
}
/* calculate an upper bound, paru, for the zero of the function. */
for (j = 0; j < n; ++j) {
sum = 0.;
for (i = 0; i <= j; ++i)
sum += r(i,j) * qtb[i];
l = ipvt[j];
wa1[j] = sum / diag[l];
}
gnorm = wa1.stableNorm();
paru = gnorm / delta;
if (paru == 0.)
paru = dwarf / std::min(delta,Scalar(0.1));
/* if the input par lies outside of the interval (parl,paru), */
/* set par to the closer endpoint. */
par = std::max(par,parl);
par = std::min(par,paru);
if (par == 0.)
par = gnorm / dxnorm;
/* beginning of an iteration. */
while (true) {
++iter;
/* evaluate the function at the current value of par. */
if (par == 0.)
par = std::max(dwarf,Scalar(.001) * paru); /* Computing MAX */
temp = ei_sqrt(par);
wa1 = temp * diag;
ipvt.cwise()+=1; // qrsolv() expects the fortran convention (as qrfac provides)
ei_qrsolv<Scalar>(n, r.data(), r.rows(), ipvt.data(), wa1.data(), qtb.data(), x.data(), sdiag.data(), wa2.data());
ipvt.cwise()-=1;
wa2 = diag.cwise() * x;
dxnorm = wa2.blueNorm();
temp = fp;
fp = dxnorm - delta;
/* if the function is small enough, accept the current value */
/* of par. also test for the exceptional cases where parl */
/* is zero or the number of iterations has reached 10. */
if (ei_abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
break;
/* compute the newton correction. */
for (j = 0; j < n; ++j) {
l = ipvt[j];
wa1[j] = diag[l] * (wa2[l] / dxnorm);
/* L180: */
}
for (j = 0; j < n; ++j) {
wa1[j] /= sdiag[j];
temp = wa1[j];
for (i = j+1; i < n; ++i)
wa1[i] -= r(i,j) * temp;
}
temp = wa1.blueNorm();
parc = fp / delta / temp / temp;
/* depending on the sign of the function, update parl or paru. */
if (fp > 0.)
parl = std::max(parl,par);
if (fp < 0.)
paru = std::min(paru,par);
/* compute an improved estimate for par. */
/* Computing MAX */
par = std::max(parl,par+parc);
/* end of an iteration. */
}
/* termination. */
if (iter == 0)
par = 0.;
return;
}

89
unsupported/Eigen/src/NonLinearOptimization/qform.h Normal file
View File

@@ -0,0 +1,89 @@
template <typename Scalar>
void ei_qform(int m, int n, Scalar *q, int
ldq, Scalar *wa)
{
/* System generated locals */
int q_dim1, q_offset;
/* Local variables */
int i, j, k, l, jm1, np1;
Scalar sum, temp;
int minmn;
/* Parameter adjustments */
--wa;
q_dim1 = ldq;
q_offset = 1 + q_dim1 * 1;
q -= q_offset;
/* Function Body */
/* zero out upper triangle of q in the first min(m,n) columns. */
minmn = std::min(m,n);
if (minmn < 2) {
goto L30;
}
for (j = 2; j <= minmn; ++j) {
jm1 = j - 1;
for (i = 1; i <= jm1; ++i) {
q[i + j * q_dim1] = 0.;
/* L10: */
}
/* L20: */
}
L30:
/* initialize remaining columns to those of the identity matrix. */
np1 = n + 1;
if (m < np1) {
goto L60;
}
for (j = np1; j <= m; ++j) {
for (i = 1; i <= m; ++i) {
q[i + j * q_dim1] = 0.;
/* L40: */
}
q[j + j * q_dim1] = 1.;
/* L50: */
}
L60:
/* accumulate q from its factored form. */
for (l = 1; l <= minmn; ++l) {
k = minmn - l + 1;
for (i = k; i <= m; ++i) {
wa[i] = q[i + k * q_dim1];
q[i + k * q_dim1] = 0.;
/* L70: */
}
q[k + k * q_dim1] = 1.;
if (wa[k] == 0.) {
goto L110;
}
for (j = k; j <= m; ++j) {
sum = 0.;
for (i = k; i <= m; ++i) {
sum += q[i + j * q_dim1] * wa[i];
/* L80: */
}
temp = sum / wa[k];
for (i = k; i <= m; ++i) {
q[i + j * q_dim1] -= temp * wa[i];
/* L90: */
}
/* L100: */
}
L110:
/* L120: */
;
}
return;
/* last card of subroutine qform. */
} /* qform_ */

136
unsupported/Eigen/src/NonLinearOptimization/qrfac.h Normal file
View File

@@ -0,0 +1,136 @@
template <typename Scalar>
void ei_qrfac(int m, int n, Scalar *a, int
lda, int pivot, int *ipvt, Scalar *rdiag,
Scalar *acnorm)
{
/* System generated locals */
int a_dim1, a_offset;
/* Local variables */
int i, j, k, jp1;
Scalar sum;
int kmax;
Scalar temp;
int minmn;
Scalar ajnorm;
Matrix< Scalar, Dynamic, 1 > wa(n+1);
/* Parameter adjustments */
--acnorm;
--rdiag;
a_dim1 = lda;
a_offset = 1 + a_dim1 * 1;
a -= a_offset;
--ipvt;
/* Function Body */
const Scalar epsmch = epsilon<Scalar>();
/* compute the initial column norms and initialize several arrays. */
for (j = 1; j <= n; ++j) {
acnorm[j] = Map< Matrix< Scalar, Dynamic, 1 > >(&a[j * a_dim1 + 1],m).blueNorm();
rdiag[j] = acnorm[j];
wa[j] = rdiag[j];
if (pivot) {
ipvt[j] = j;
}
/* L10: */
}
/* reduce a to r with householder transformations. */
minmn = std::min(m,n);
for (j = 1; j <= minmn; ++j) {
if (! (pivot)) {
goto L40;
}
/* bring the column of largest norm into the pivot position. */
kmax = j;
for (k = j; k <= n; ++k) {
if (rdiag[k] > rdiag[kmax]) {
kmax = k;
}
/* L20: */
}
if (kmax == j) {
goto L40;
}
for (i = 1; i <= m; ++i) {
temp = a[i + j * a_dim1];
a[i + j * a_dim1] = a[i + kmax * a_dim1];
a[i + kmax * a_dim1] = temp;
/* L30: */
}
rdiag[kmax] = rdiag[j];
wa[kmax] = wa[j];
k = ipvt[j];
ipvt[j] = ipvt[kmax];
ipvt[kmax] = k;
L40:
/* compute the householder transformation to reduce the */
/* j-th column of a to a multiple of the j-th unit vector. */
ajnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&a[j + j * a_dim1],m-j+1).blueNorm();
if (ajnorm == 0.) {
goto L100;
}
if (a[j + j * a_dim1] < 0.) {
ajnorm = -ajnorm;
}
for (i = j; i <= m; ++i) {
a[i + j * a_dim1] /= ajnorm;
/* L50: */
}
a[j + j * a_dim1] += 1.;
/* apply the transformation to the remaining columns */
/* and update the norms. */
jp1 = j + 1;
if (n < jp1) {
goto L100;
}
for (k = jp1; k <= n; ++k) {
sum = 0.;
for (i = j; i <= m; ++i) {
sum += a[i + j * a_dim1] * a[i + k * a_dim1];
/* L60: */
}
temp = sum / a[j + j * a_dim1];
for (i = j; i <= m; ++i) {
a[i + k * a_dim1] -= temp * a[i + j * a_dim1];
/* L70: */
}
if (! (pivot) || rdiag[k] == 0.) {
goto L80;
}
temp = a[j + k * a_dim1] / rdiag[k];
/* Computing MAX */
/* Computing 2nd power */
rdiag[k] *= ei_sqrt((std::max(Scalar(0.), Scalar(1.)-ei_abs2(temp))));
/* Computing 2nd power */
if (Scalar(.05) * ei_abs2(rdiag[k] / wa[k]) > epsmch) {
goto L80;
}
rdiag[k] = Map< Matrix< Scalar, Dynamic, 1 > >(&a[jp1 + k * a_dim1],m-j).blueNorm();
wa[k] = rdiag[k];
L80:
/* L90: */
;
}
L100:
rdiag[j] = -ajnorm;
/* L110: */
}
return;
/* last card of subroutine qrfac. */
} /* qrfac_ */

166
unsupported/Eigen/src/NonLinearOptimization/qrsolv.h Normal file
View File

@@ -0,0 +1,166 @@
template <typename Scalar>
void ei_qrsolv(int n, Scalar *r__, int ldr,
const int *ipvt, const Scalar *diag, const Scalar *qtb, Scalar *x,
Scalar *sdiag, Scalar *wa)
{
/* System generated locals */
int r_dim1, r_offset;
/* Local variables */
int i, j, k, l, jp1, kp1;
Scalar tan__, cos__, sin__, sum, temp, cotan;
int nsing;
Scalar qtbpj;
/* Parameter adjustments */
--wa;
--sdiag;
--x;
--qtb;
--diag;
--ipvt;
r_dim1 = ldr;
r_offset = 1 + r_dim1 * 1;
r__ -= r_offset;
/* Function Body */
/* copy r and (q transpose)*b to preserve input and initialize s. */
/* in particular, save the diagonal elements of r in x. */
for (j = 1; j <= n; ++j) {
for (i = j; i <= n; ++i) {
r__[i + j * r_dim1] = r__[j + i * r_dim1];
/* L10: */
}
x[j] = r__[j + j * r_dim1];
wa[j] = qtb[j];
/* L20: */
}
/* eliminate the diagonal matrix d using a givens rotation. */
for (j = 1; j <= n; ++j) {
/* prepare the row of d to be eliminated, locating the */
/* diagonal element using p from the qr factorization. */
l = ipvt[j];
if (diag[l] == 0.) {
goto L90;
}
for (k = j; k <= n; ++k) {
sdiag[k] = 0.;
/* L30: */
}
sdiag[j] = diag[l];
/* the transformations to eliminate the row of d */
/* modify only a single element of (q transpose)*b */
/* beyond the first n, which is initially zero. */
qtbpj = 0.;
for (k = j; k <= n; ++k) {
/* determine a givens rotation which eliminates the */
/* appropriate element in the current row of d. */
if (sdiag[k] == 0.)
goto L70;
if ( ei_abs(r__[k + k * r_dim1]) >= ei_abs(sdiag[k]))
goto L40;
cotan = r__[k + k * r_dim1] / sdiag[k];
/* Computing 2nd power */
sin__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
cos__ = sin__ * cotan;
goto L50;
L40:
tan__ = sdiag[k] / r__[k + k * r_dim1];
/* Computing 2nd power */
cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
sin__ = cos__ * tan__;
L50:
/* compute the modified diagonal element of r and */
/* the modified element of ((q transpose)*b,0). */
r__[k + k * r_dim1] = cos__ * r__[k + k * r_dim1] + sin__ * sdiag[
k];
temp = cos__ * wa[k] + sin__ * qtbpj;
qtbpj = -sin__ * wa[k] + cos__ * qtbpj;
wa[k] = temp;
/* accumulate the tranformation in the row of s. */
kp1 = k + 1;
if (n < kp1) {
goto L70;
}
for (i = kp1; i <= n; ++i) {
temp = cos__ * r__[i + k * r_dim1] + sin__ * sdiag[i];
sdiag[i] = -sin__ * r__[i + k * r_dim1] + cos__ * sdiag[
i];
r__[i + k * r_dim1] = temp;
/* L60: */
}
L70:
/* L80: */
;
}
L90:
/* store the diagonal element of s and restore */
/* the corresponding diagonal element of r. */
sdiag[j] = r__[j + j * r_dim1];
r__[j + j * r_dim1] = x[j];
/* L100: */
}
/* solve the triangular system for z. if the system is */
/* singular, then obtain a least squares solution. */
nsing = n;
for (j = 1; j <= n; ++j) {
if (sdiag[j] == 0. && nsing == n) {
nsing = j - 1;
}
if (nsing < n) {
wa[j] = 0.;
}
/* L110: */
}
if (nsing < 1) {
goto L150;
}
for (k = 1; k <= nsing; ++k) {
j = nsing - k + 1;
sum = 0.;
jp1 = j + 1;
if (nsing < jp1) {
goto L130;
}
for (i = jp1; i <= nsing; ++i) {
sum += r__[i + j * r_dim1] * wa[i];
/* L120: */
}
L130:
wa[j] = (wa[j] - sum) / sdiag[j];
/* L140: */
}
L150:
/* permute the components of z back to components of x. */
for (j = 1; j <= n; ++j) {
l = ipvt[j];
x[l] = wa[j];
/* L160: */
}
return;
/* last card of subroutine qrsolv. */
} /* qrsolv_ */

87
unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h Normal file
View File

@@ -0,0 +1,87 @@
template <typename Scalar>
void ei_r1mpyq(int m, int n, Scalar *a, int
lda, const Scalar *v, const Scalar *w)
{
/* System generated locals */
int a_dim1, a_offset;
/* Local variables */
int i, j, nm1, nmj;
Scalar cos__, sin__, temp;
/* Parameter adjustments */
--w;
--v;
a_dim1 = lda;
a_offset = 1 + a_dim1 * 1;
a -= a_offset;
/* Function Body */
/* apply the first set of givens rotations to a. */
nm1 = n - 1;
if (nm1 < 1) {
/* goto L50; */
return;
}
for (nmj = 1; nmj <= nm1; ++nmj) {
j = n - nmj;
if (ei_abs(v[j]) > 1.) {
cos__ = 1. / v[j];
}
if (ei_abs(v[j]) > 1.) {
/* Computing 2nd power */
sin__ = ei_sqrt(1. - ei_abs2(cos__));
}
if (ei_abs(v[j]) <= 1.) {
sin__ = v[j];
}
if (ei_abs(v[j]) <= 1.) {
/* Computing 2nd power */
cos__ = ei_sqrt(1. - ei_abs2(sin__));
}
for (i = 1; i <= m; ++i) {
temp = cos__ * a[i + j * a_dim1] - sin__ * a[i + n * a_dim1];
a[i + n * a_dim1] = sin__ * a[i + j * a_dim1] + cos__ * a[
i + n * a_dim1];
a[i + j * a_dim1] = temp;
/* L10: */
}
/* L20: */
}
/* apply the second set of givens rotations to a. */
for (j = 1; j <= nm1; ++j) {
if (ei_abs(w[j]) > 1.) {
cos__ = 1. / w[j];
}
if (ei_abs(w[j]) > 1.) {
/* Computing 2nd power */
sin__ = ei_sqrt(1. - ei_abs2(cos__));
}
if (ei_abs(w[j]) <= 1.) {
sin__ = w[j];
}
if (ei_abs(w[j]) <= 1.) {
/* Computing 2nd power */
cos__ = ei_sqrt(1. - ei_abs2(sin__));
}
for (i = 1; i <= m; ++i) {
temp = cos__ * a[i + j * a_dim1] + sin__ * a[i + n * a_dim1];
a[i + n * a_dim1] = -sin__ * a[i + j * a_dim1] + cos__ * a[
i + n * a_dim1];
a[i + j * a_dim1] = temp;
/* L30: */
}
/* L40: */
}
/* L50: */
return;
/* last card of subroutine r1mpyq. */
} /* r1mpyq_ */

175
unsupported/Eigen/src/NonLinearOptimization/r1updt.h Normal file
View File

@@ -0,0 +1,175 @@
template <typename Scalar>
void ei_r1updt(int m, int n, Scalar *s, int /* ls */, const Scalar *u, Scalar *v, Scalar *w, bool *sing)
{
/* Local variables */
int i, j, l, jj, nm1;
Scalar tan__;
int nmj;
Scalar cos__, sin__, tau, temp, cotan;
/* Parameter adjustments */
--w;
--u;
--v;
--s;
/* Function Body */
const Scalar giant = std::numeric_limits<Scalar>::max();
/* initialize the diagonal element pointer. */
jj = n * ((m << 1) - n + 1) / 2 - (m - n);
/* move the nontrivial part of the last column of s into w. */
l = jj;
for (i = n; i <= m; ++i) {
w[i] = s[l];
++l;
/* L10: */
}
/* rotate the vector v into a multiple of the n-th unit vector */
/* in such a way that a spike is introduced into w. */
nm1 = n - 1;
if (nm1 < 1) {
goto L70;
}
for (nmj = 1; nmj <= nm1; ++nmj) {
j = n - nmj;
jj -= m - j + 1;
w[j] = 0.;
if (v[j] == 0.) {
goto L50;
}
/* determine a givens rotation which eliminates the */
/* j-th element of v. */
if (ei_abs(v[n]) >= ei_abs(v[j]))
goto L20;
cotan = v[n] / v[j];
/* Computing 2nd power */
sin__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
cos__ = sin__ * cotan;
tau = 1.;
if (ei_abs(cos__) * giant > 1.) {
tau = 1. / cos__;
}
goto L30;
L20:
tan__ = v[j] / v[n];
/* Computing 2nd power */
cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
sin__ = cos__ * tan__;
tau = sin__;
L30:
/* apply the transformation to v and store the information */
/* necessary to recover the givens rotation. */
v[n] = sin__ * v[j] + cos__ * v[n];
v[j] = tau;
/* apply the transformation to s and extend the spike in w. */
l = jj;
for (i = j; i <= m; ++i) {
temp = cos__ * s[l] - sin__ * w[i];
w[i] = sin__ * s[l] + cos__ * w[i];
s[l] = temp;
++l;
/* L40: */
}
L50:
/* L60: */
;
}
L70:
/* add the spike from the rank 1 update to w. */
for (i = 1; i <= m; ++i) {
w[i] += v[n] * u[i];
/* L80: */
}
/* eliminate the spike. */
*sing = false;
if (nm1 < 1) {
goto L140;
}
for (j = 1; j <= nm1; ++j) {
if (w[j] == 0.) {
goto L120;
}
/* determine a givens rotation which eliminates the */
/* j-th element of the spike. */
if (ei_abs(s[jj]) >= ei_abs(w[j]))
goto L90;
cotan = s[jj] / w[j];
/* Computing 2nd power */
sin__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
cos__ = sin__ * cotan;
tau = 1.;
if (ei_abs(cos__) * giant > 1.) {
tau = 1. / cos__;
}
goto L100;
L90:
tan__ = w[j] / s[jj];
/* Computing 2nd power */
cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
sin__ = cos__ * tan__;
tau = sin__;
L100:
/* apply the transformation to s and reduce the spike in w. */
l = jj;
for (i = j; i <= m; ++i) {
temp = cos__ * s[l] + sin__ * w[i];
w[i] = -sin__ * s[l] + cos__ * w[i];
s[l] = temp;
++l;
/* L110: */
}
/* store the information necessary to recover the */
/* givens rotation. */
w[j] = tau;
L120:
/* test for zero diagonal elements in the output s. */
if (s[jj] == 0.) {
*sing = true;
}
jj += m - j + 1;
/* L130: */
}
L140:
/* move w back into the last column of the output s. */
l = jj;
for (i = n; i <= m; ++i) {
s[l] = w[i];
++l;
/* L150: */
}
if (s[jj] == 0.) {
*sing = true;
}
return;
/* last card of subroutine r1updt. */
} /* r1updt_ */

80
unsupported/Eigen/src/NonLinearOptimization/rwupdt.h Normal file
View File

@@ -0,0 +1,80 @@
template <typename Scalar>
void ei_rwupdt(int n, Scalar *r__, int ldr,
const Scalar *w, Scalar *b, Scalar *alpha, Scalar *cos__,
Scalar *sin__)
{
/* System generated locals */
int r_dim1, r_offset;
/* Local variables */
int i, j, jm1;
Scalar tan__, temp, rowj, cotan;
/* Parameter adjustments */
--sin__;
--cos__;
--b;
--w;
r_dim1 = ldr;
r_offset = 1 + r_dim1 * 1;
r__ -= r_offset;
/* Function Body */
for (j = 1; j <= n; ++j) {
rowj = w[j];
jm1 = j - 1;
/* apply the previous transformations to */
/* r(i,j), i=1,2,...,j-1, and to w(j). */
if (jm1 < 1) {
goto L20;
}
for (i = 1; i <= jm1; ++i) {
temp = cos__[i] * r__[i + j * r_dim1] + sin__[i] * rowj;
rowj = -sin__[i] * r__[i + j * r_dim1] + cos__[i] * rowj;
r__[i + j * r_dim1] = temp;
/* L10: */
}
L20:
/* determine a givens rotation which eliminates w(j). */
cos__[j] = 1.;
sin__[j] = 0.;
if (rowj == 0.) {
goto L50;
}
if (ei_abs(r__[j + j * r_dim1]) >= ei_abs(rowj))
goto L30;
cotan = r__[j + j * r_dim1] / rowj;
/* Computing 2nd power */
sin__[j] = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
cos__[j] = sin__[j] * cotan;
goto L40;
L30:
tan__ = rowj / r__[j + j * r_dim1];
/* Computing 2nd power */
cos__[j] = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
sin__[j] = cos__[j] * tan__;
L40:
/* apply the current transformation to r(j,j), b(j), and alpha. */
r__[j + j * r_dim1] = cos__[j] * r__[j + j * r_dim1] + sin__[j] *
rowj;
temp = cos__[j] * b[j] + sin__[j] * *alpha;
*alpha = -sin__[j] * b[j] + cos__[j] * *alpha;
b[j] = temp;
L50:
/* L60: */
;
}
return;
/* last card of subroutine rwupdt. */
} /* rwupdt_ */