GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-08-12 11:49:02 +08:00
Code Issues Packages Projects Releases Wiki Activity

some more eigenization

Browse Source
This commit is contained in:
Thomas Capricelli 2010-01-26 07:36:15 +01:00
parent a3034ee079
commit ba2a9cce03
2 changed files with 39 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
View File

@ -186,7 +186,6 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveInit(
njev = 0; njev = 0;
/* check the input parameters for errors. */ /* check the input parameters for errors. */
if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0. ) if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0. )
return ImproperInputParameters; return ImproperInputParameters;
if (mode == 2) if (mode == 2)
@ -196,14 +195,12 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveInit(
/* evaluate the function at the starting point */ /* evaluate the function at the starting point */
/* and calculate its norm. */ /* and calculate its norm. */
nfev = 1; nfev = 1;
if ( functor(x, fvec) < 0) if ( functor(x, fvec) < 0)
return UserAksed; return UserAksed;
fnorm = fvec.stableNorm(); fnorm = fvec.stableNorm();
/* initialize iteration counter and monitors. */ /* initialize iteration counter and monitors. */
iter = 1; iter = 1;
ncsuc = 0; ncsuc = 0;
ncfail = 0; ncfail = 0;
@ -224,7 +221,6 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
jeval = true; jeval = true;
/* calculate the jacobian matrix. */ /* calculate the jacobian matrix. */
if ( functor.df(x, fjac) < 0) if ( functor.df(x, fjac) < 0)
return UserAksed; return UserAksed;
++njev; ++njev;
@ -235,11 +231,8 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
/* to the norms of the columns of the initial jacobian. */ /* to the norms of the columns of the initial jacobian. */
if (iter == 1) { if (iter == 1) {
if (mode != 2) if (mode != 2)
for (j = 0; j < n; ++j) { for (j = 0; j < n; ++j)
diag[j] = wa2[j]; diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
if (wa2[j] == 0.)
diag[j] = 1.;
}
/* on the first iteration, calculate the norm of the scaled x */ /* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */ /* and initialize the step bound delta. */
@ -260,7 +253,6 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
for(int ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors for(int ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors
/* form (q transpose)*fvec and store in qtf. */ /* form (q transpose)*fvec and store in qtf. */
qtf = fvec; qtf = fvec;
for (j = 0; j < n; ++j) for (j = 0; j < n; ++j)
if (fjac(j,j) != 0.) { if (fjac(j,j) != 0.) {
@ -273,76 +265,54 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
} }
/* copy the triangular factor of the qr factorization into r. */ /* copy the triangular factor of the qr factorization into r. */
R = qrfac.matrixQR(); R = qrfac.matrixQR();
sing = false; sing = wa1.cwiseAbs().minCoeff()==0.;
for (j = 0; j < n; ++j)
if (wa1[j] == 0.) sing = true;
/* accumulate the orthogonal factor in fjac. */ /* accumulate the orthogonal factor in fjac. */
ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data()); ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data());
/* rescale if necessary. */ /* rescale if necessary. */
/* Computing MAX */
if (mode != 2) if (mode != 2)
diag = diag.cwiseMax(wa2); diag = diag.cwiseMax(wa2);
/* beginning of the inner loop. */
while (true) { while (true) {
/* determine the direction p. */ /* determine the direction p. */
ei_dogleg<Scalar>(R, diag, qtf, delta, wa1); ei_dogleg<Scalar>(R, diag, qtf, delta, wa1);
/* store the direction p and x + p. calculate the norm of p. */ /* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1; wa1 = -wa1;
wa2 = x + wa1; wa2 = x + wa1;
wa3 = diag.cwiseProduct(wa1); wa3 = diag.cwiseProduct(wa1);
pnorm = wa3.stableNorm(); pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */ /* on the first iteration, adjust the initial step bound. */
if (iter == 1) if (iter == 1)
delta = std::min(delta,pnorm); delta = std::min(delta,pnorm);
/* evaluate the function at x + p and calculate its norm. */ /* evaluate the function at x + p and calculate its norm. */
if ( functor(wa2, wa4) < 0) if ( functor(wa2, wa4) < 0)
return UserAksed; return UserAksed;
++nfev; ++nfev;
fnorm1 = wa4.stableNorm(); fnorm1 = wa4.stableNorm();
/* compute the scaled actual reduction. */ /* compute the scaled actual reduction. */
actred = -1.; actred = -1.;
if (fnorm1 < fnorm) /* Computing 2nd power */ if (fnorm1 < fnorm) /* Computing 2nd power */
actred = 1. - ei_abs2(fnorm1 / fnorm); actred = 1. - ei_abs2(fnorm1 / fnorm);
/* compute the scaled predicted reduction. */ /* compute the scaled predicted reduction. */
wa3 = R.template triangularView<Upper>()*wa1 + qtf;
for (i = 0; i < n; ++i) {
sum = 0.;
for (j = i; j < n; ++j)
sum += R(i,j) * wa1[j];
wa3[i] = qtf[i] + sum;
}
temp = wa3.stableNorm(); temp = wa3.stableNorm();
prered = 0.; prered = 0.;
if (temp < fnorm) /* Computing 2nd power */ if (temp < fnorm) /* Computing 2nd power */
prered = 1. - ei_abs2(temp / fnorm); prered = 1. - ei_abs2(temp / fnorm);
/* compute the ratio of the actual to the predicted */ /* compute the ratio of the actual to the predicted reduction. */
/* reduction. */
ratio = 0.; ratio = 0.;
if (prered > 0.) if (prered > 0.)
ratio = actred / prered; ratio = actred / prered;
/* update the step bound. */ /* update the step bound. */
if (ratio < Scalar(.1)) { if (ratio < Scalar(.1)) {
ncsuc = 0; ncsuc = 0;
++ncfail; ++ncfail;
@ -350,7 +320,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
} else { } else {
ncfail = 0; ncfail = 0;
++ncsuc; ++ncsuc;
if (ratio >= Scalar(.5) || ncsuc > 1) /* Computing MAX */ if (ratio >= Scalar(.5) || ncsuc > 1)
delta = std::max(delta, pnorm / Scalar(.5)); delta = std::max(delta, pnorm / Scalar(.5));
if (ei_abs(ratio - 1.) <= Scalar(.1)) { if (ei_abs(ratio - 1.) <= Scalar(.1)) {
delta = pnorm / Scalar(.5); delta = pnorm / Scalar(.5);
@ -358,7 +328,6 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
} }
/* test for successful iteration. */ /* test for successful iteration. */
if (ratio >= Scalar(1e-4)) { if (ratio >= Scalar(1e-4)) {
/* successful iteration. update x, fvec, and their norms. */ /* successful iteration. update x, fvec, and their norms. */
x = wa2; x = wa2;
@ -370,7 +339,6 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
} }
/* determine the progress of the iteration. */ /* determine the progress of the iteration. */
++nslow1; ++nslow1;
if (actred >= Scalar(.001)) if (actred >= Scalar(.001))
nslow1 = 0; nslow1 = 0;
@ -380,12 +348,10 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
nslow2 = 0; nslow2 = 0;
/* test for convergence. */ /* test for convergence. */
if (delta <= parameters.xtol * xnorm || fnorm == 0.) if (delta <= parameters.xtol * xnorm || fnorm == 0.)
return RelativeErrorTooSmall; return RelativeErrorTooSmall;
/* tests for termination and stringent tolerances. */ /* tests for termination and stringent tolerances. */
if (nfev >= parameters.maxfev) if (nfev >= parameters.maxfev)
return TooManyFunctionEvaluation; return TooManyFunctionEvaluation;
if (Scalar(.1) * std::max(Scalar(.1) * delta, pnorm) <= epsilon<Scalar>() * xnorm) if (Scalar(.1) * std::max(Scalar(.1) * delta, pnorm) <= epsilon<Scalar>() * xnorm)
@ -396,37 +362,27 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
return NotMakingProgressIterations; return NotMakingProgressIterations;
/* criterion for recalculating jacobian. */ /* criterion for recalculating jacobian. */
if (ncfail == 2) if (ncfail == 2)
break; // leave inner loop and go for the next outer loop iteration break; // leave inner loop and go for the next outer loop iteration
/* calculate the rank one modification to the jacobian */ /* calculate the rank one modification to the jacobian */
/* and update qtf if necessary. */ /* and update qtf if necessary. */
wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
for (j = 0; j < n; ++j) { wa2 = fjac.transpose() * wa4;
sum = wa4.dot(fjac.col(j)); if (ratio >= Scalar(1e-4))
wa2[j] = (sum - wa3[j]) / pnorm; qtf = wa2;
wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm); wa2 = (wa2-wa3)/pnorm;
if (ratio >= Scalar(1e-4))
qtf[j] = sum;
}
/* compute the qr factorization of the updated jacobian. */ /* compute the qr factorization of the updated jacobian. */
ei_r1updt<Scalar>(n, n, R, wa1.data(), wa2.data(), wa3.data(), &sing); ei_r1updt<Scalar>(n, n, R, wa1.data(), wa2.data(), wa3.data(), &sing);
ei_r1mpyq<Scalar>(n, n, fjac.data(), fjac.rows(), wa2.data(), wa3.data()); 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()); ei_r1mpyq<Scalar>(1, n, qtf.data(), 1, wa2.data(), wa3.data());
/* end of the inner loop. */
jeval = false; jeval = false;
} }
/* end of the outer loop. */
return Running; return Running;
} }
template<typename FunctorType, typename Scalar> template<typename FunctorType, typename Scalar>
typename HybridNonLinearSolver<FunctorType,Scalar>::Status typename HybridNonLinearSolver<FunctorType,Scalar>::Status
HybridNonLinearSolver<FunctorType,Scalar>::solve( HybridNonLinearSolver<FunctorType,Scalar>::solve(
@ -493,7 +449,6 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(
njev = 0; njev = 0;
/* check the input parameters for errors. */ /* 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. ) if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. )
return ImproperInputParameters; return ImproperInputParameters;
if (mode == 2) if (mode == 2)
@ -503,14 +458,12 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(
/* evaluate the function at the starting point */ /* evaluate the function at the starting point */
/* and calculate its norm. */ /* and calculate its norm. */
nfev = 1; nfev = 1;
if ( functor(x, fvec) < 0) if ( functor(x, fvec) < 0)
return UserAksed; return UserAksed;
fnorm = fvec.stableNorm(); fnorm = fvec.stableNorm();
/* initialize iteration counter and monitors. */ /* initialize iteration counter and monitors. */
iter = 1; iter = 1;
ncsuc = 0; ncsuc = 0;
ncfail = 0; ncfail = 0;
@ -544,11 +497,8 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
/* to the norms of the columns of the initial jacobian. */ /* to the norms of the columns of the initial jacobian. */
if (iter == 1) { if (iter == 1) {
if (mode != 2) if (mode != 2)
for (j = 0; j < n; ++j) { for (j = 0; j < n; ++j)
diag[j] = wa2[j]; diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
if (wa2[j] == 0.)
diag[j] = 1.;
}
/* on the first iteration, calculate the norm of the scaled x */ /* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */ /* and initialize the step bound delta. */
@ -569,7 +519,6 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
for(int ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors for(int ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors
/* form (q transpose)*fvec and store in qtf. */ /* form (q transpose)*fvec and store in qtf. */
qtf = fvec; qtf = fvec;
for (j = 0; j < n; ++j) for (j = 0; j < n; ++j)
if (fjac(j,j) != 0.) { if (fjac(j,j) != 0.) {
@ -583,74 +532,53 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
/* copy the triangular factor of the qr factorization into r. */ /* copy the triangular factor of the qr factorization into r. */
R = qrfac.matrixQR(); R = qrfac.matrixQR();
sing = false; sing = wa1.cwiseAbs().minCoeff()==0.;
for (j = 0; j < n; ++j)
if (wa1[j] == 0.) sing = true;
/* accumulate the orthogonal factor in fjac. */ /* accumulate the orthogonal factor in fjac. */
ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data()); ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data());
/* rescale if necessary. */ /* rescale if necessary. */
/* Computing MAX */
if (mode != 2) if (mode != 2)
diag = diag.cwiseMax(wa2); diag = diag.cwiseMax(wa2);
/* beginning of the inner loop. */
while (true) { while (true) {
/* determine the direction p. */ /* determine the direction p. */
ei_dogleg<Scalar>(R, diag, qtf, delta, wa1); ei_dogleg<Scalar>(R, diag, qtf, delta, wa1);
/* store the direction p and x + p. calculate the norm of p. */ /* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1; wa1 = -wa1;
wa2 = x + wa1; wa2 = x + wa1;
wa3 = diag.cwiseProduct(wa1); wa3 = diag.cwiseProduct(wa1);
pnorm = wa3.stableNorm(); pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */ /* on the first iteration, adjust the initial step bound. */
if (iter == 1) if (iter == 1)
delta = std::min(delta,pnorm); delta = std::min(delta,pnorm);
/* evaluate the function at x + p and calculate its norm. */ /* evaluate the function at x + p and calculate its norm. */
if ( functor(wa2, wa4) < 0) if ( functor(wa2, wa4) < 0)
return UserAksed; return UserAksed;
++nfev; ++nfev;
fnorm1 = wa4.stableNorm(); fnorm1 = wa4.stableNorm();
/* compute the scaled actual reduction. */ /* compute the scaled actual reduction. */
actred = -1.; actred = -1.;
if (fnorm1 < fnorm) /* Computing 2nd power */ if (fnorm1 < fnorm) /* Computing 2nd power */
actred = 1. - ei_abs2(fnorm1 / fnorm); actred = 1. - ei_abs2(fnorm1 / fnorm);
/* compute the scaled predicted reduction. */ /* compute the scaled predicted reduction. */
wa3 = R.template triangularView<Upper>()*wa1 + qtf;
for (i = 0; i < n; ++i) {
sum = 0.;
for (j = i; j < n; ++j)
sum += R(i,j) * wa1[j];
wa3[i] = qtf[i] + sum;
}
temp = wa3.stableNorm(); temp = wa3.stableNorm();
prered = 0.; prered = 0.;
if (temp < fnorm) /* Computing 2nd power */ if (temp < fnorm) /* Computing 2nd power */
prered = 1. - ei_abs2(temp / fnorm); prered = 1. - ei_abs2(temp / fnorm);
/* compute the ratio of the actual to the predicted */ /* compute the ratio of the actual to the predicted reduction. */
/* reduction. */
ratio = 0.; ratio = 0.;
if (prered > 0.) if (prered > 0.)
ratio = actred / prered; ratio = actred / prered;
/* update the step bound. */ /* update the step bound. */
if (ratio < Scalar(.1)) { if (ratio < Scalar(.1)) {
ncsuc = 0; ncsuc = 0;
++ncfail; ++ncfail;
@ -658,7 +586,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
} else { } else {
ncfail = 0; ncfail = 0;
++ncsuc; ++ncsuc;
if (ratio >= Scalar(.5) || ncsuc > 1) /* Computing MAX */ if (ratio >= Scalar(.5) || ncsuc > 1)
delta = std::max(delta, pnorm / Scalar(.5)); delta = std::max(delta, pnorm / Scalar(.5));
if (ei_abs(ratio - 1.) <= Scalar(.1)) { if (ei_abs(ratio - 1.) <= Scalar(.1)) {
delta = pnorm / Scalar(.5); delta = pnorm / Scalar(.5);
@ -666,7 +594,6 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
} }
/* test for successful iteration. */ /* test for successful iteration. */
if (ratio >= Scalar(1e-4)) { if (ratio >= Scalar(1e-4)) {
/* successful iteration. update x, fvec, and their norms. */ /* successful iteration. update x, fvec, and their norms. */
x = wa2; x = wa2;
@ -678,7 +605,6 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
} }
/* determine the progress of the iteration. */ /* determine the progress of the iteration. */
++nslow1; ++nslow1;
if (actred >= Scalar(.001)) if (actred >= Scalar(.001))
nslow1 = 0; nslow1 = 0;
@ -688,12 +614,10 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
nslow2 = 0; nslow2 = 0;
/* test for convergence. */ /* test for convergence. */
if (delta <= parameters.xtol * xnorm || fnorm == 0.) if (delta <= parameters.xtol * xnorm || fnorm == 0.)
return RelativeErrorTooSmall; return RelativeErrorTooSmall;
/* tests for termination and stringent tolerances. */ /* tests for termination and stringent tolerances. */
if (nfev >= parameters.maxfev) if (nfev >= parameters.maxfev)
return TooManyFunctionEvaluation; return TooManyFunctionEvaluation;
if (Scalar(.1) * std::max(Scalar(.1) * delta, pnorm) <= epsilon<Scalar>() * xnorm) if (Scalar(.1) * std::max(Scalar(.1) * delta, pnorm) <= epsilon<Scalar>() * xnorm)
@ -703,35 +627,25 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
if (nslow1 == 10) if (nslow1 == 10)
return NotMakingProgressIterations; return NotMakingProgressIterations;
/* criterion for recalculating jacobian approximation */ /* criterion for recalculating jacobian. */
/* by forward differences. */
if (ncfail == 2) if (ncfail == 2)
break; // leave inner loop and go for the next outer loop iteration break; // leave inner loop and go for the next outer loop iteration
/* calculate the rank one modification to the jacobian */ /* calculate the rank one modification to the jacobian */
/* and update qtf if necessary. */ /* and update qtf if necessary. */
wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
for (j = 0; j < n; ++j) { wa2 = fjac.transpose() * wa4;
sum = wa4.dot(fjac.col(j)); if (ratio >= Scalar(1e-4))
wa2[j] = (sum - wa3[j]) / pnorm; qtf = wa2;
wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm); wa2 = (wa2-wa3)/pnorm;
if (ratio >= Scalar(1e-4))
qtf[j] = sum;
}
/* compute the qr factorization of the updated jacobian. */ /* compute the qr factorization of the updated jacobian. */
ei_r1updt<Scalar>(n, n, R, wa1.data(), wa2.data(), wa3.data(), &sing); ei_r1updt<Scalar>(n, n, R, wa1.data(), wa2.data(), wa3.data(), &sing);
ei_r1mpyq<Scalar>(n, n, fjac.data(), fjac.rows(), wa2.data(), wa3.data()); 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()); ei_r1mpyq<Scalar>(1, n, qtf.data(), 1, wa2.data(), wa3.data());
/* end of the inner loop. */
jeval = false; jeval = false;
} }
/* end of the outer loop. */
return Running; return Running;
} }

38
unsupported/Eigen/src/NonLinearOptimization/dogleg.h
View File

@ -8,46 +8,45 @@ void ei_dogleg(
Matrix< Scalar, Dynamic, 1 > &x) Matrix< Scalar, Dynamic, 1 > &x)
{ {
/* Local variables */ /* Local variables */
int i, j, k; int i, j;
Scalar sum, temp, alpha, bnorm; Scalar sum, temp, alpha, bnorm;
Scalar gnorm, qnorm; Scalar gnorm, qnorm;
Scalar sgnorm; Scalar sgnorm;
/* Function Body */ /* Function Body */
const Scalar epsmch = epsilon<Scalar>(); const Scalar epsmch = epsilon<Scalar>();
const int n = diag.size(); const int n = qrfac.cols();
Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n);
assert(n==qtb.size()); assert(n==qtb.size());
assert(n==x.size()); assert(n==x.size());
assert(n==diag.size());
Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n);
for (k = 0; k < n; ++k) { /* first, calculate the gauss-newton direction. */
j = n - k - 1; for (j = n-1; j >=0; --j) {
sum = 0.;
for (i = j+1; i < n; ++i) {
sum += qrfac(j,i) * x[i];
}
temp = qrfac(j,j); temp = qrfac(j,j);
if (temp == 0.) { if (temp == 0.) {
for (i = 0; i <= j; ++i) temp = epsmch * qrfac.col(j).head(j+1).maxCoeff();
temp = std::max(temp,ei_abs(qrfac(i,j)));
temp = epsmch * temp;
if (temp == 0.) if (temp == 0.)
temp = epsmch; temp = epsmch;
} }
x[j] = (qtb[j] - sum) / temp; if (j==n-1)
x[j] = qtb[j] / temp;
else
x[j] = (qtb[j] - qrfac.row(j).tail(n-j-1).dot(x.tail(n-j-1))) / temp;
} }
/* test whether the gauss-newton direction is acceptable. */ /* test whether the gauss-newton direction is acceptable. */
wa1.fill(0.);
wa2 = diag.cwiseProduct(x); wa2 = diag.cwiseProduct(x);
qnorm = wa2.stableNorm(); qnorm = wa2.stableNorm();
if (qnorm <= delta) if (qnorm <= delta)
return; return;
// TODO : this path is not tested by Eigen unit tests
/* the gauss-newton direction is not acceptable. */ /* the gauss-newton direction is not acceptable. */
/* next, calculate the scaled gradient direction. */ /* next, calculate the scaled gradient direction. */
wa1.fill(0.);
for (j = 0; j < n; ++j) { for (j = 0; j < n; ++j) {
temp = qtb[j]; temp = qtb[j];
for (i = j; i < n; ++i) for (i = j; i < n; ++i)
@ -57,7 +56,6 @@ void ei_dogleg(
/* calculate the norm of the scaled gradient and test for */ /* calculate the norm of the scaled gradient and test for */
/* the special case in which the scaled gradient is zero. */ /* the special case in which the scaled gradient is zero. */
gnorm = wa1.stableNorm(); gnorm = wa1.stableNorm();
sgnorm = 0.; sgnorm = 0.;
alpha = delta / qnorm; alpha = delta / qnorm;
@ -66,8 +64,9 @@ void ei_dogleg(
/* calculate the point along the scaled gradient */ /* calculate the point along the scaled gradient */
/* at which the quadratic is minimized. */ /* at which the quadratic is minimized. */
wa1.array() /= (diag*gnorm).array(); wa1.array() /= (diag*gnorm).array();
// TODO : once unit tests cover this part,:
// wa2 = qrfac.template triangularView<Upper>() * wa1;
for (j = 0; j < n; ++j) { for (j = 0; j < n; ++j) {
sum = 0.; sum = 0.;
for (i = j; i < n; ++i) { for (i = j; i < n; ++i) {
@ -79,7 +78,6 @@ void ei_dogleg(
sgnorm = gnorm / temp / temp; sgnorm = gnorm / temp / temp;
/* test whether the scaled gradient direction is acceptable. */ /* test whether the scaled gradient direction is acceptable. */
alpha = 0.; alpha = 0.;
if (sgnorm >= delta) if (sgnorm >= delta)
goto algo_end; goto algo_end;
@ -87,18 +85,14 @@ void ei_dogleg(
/* the scaled gradient direction is not acceptable. */ /* the scaled gradient direction is not acceptable. */
/* finally, calculate the point along the dogleg */ /* finally, calculate the point along the dogleg */
/* at which the quadratic is minimized. */ /* at which the quadratic is minimized. */
bnorm = qtb.stableNorm(); bnorm = qtb.stableNorm();
temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta); 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))); 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; alpha = delta / qnorm * (1. - ei_abs2(sgnorm / delta)) / temp;
algo_end: algo_end:
/* form appropriate convex combination of the gauss-newton */ /* form appropriate convex combination of the gauss-newton */
/* direction and the scaled gradient direction. */ /* direction and the scaled gradient direction. */
temp = (1.-alpha) * std::min(sgnorm,delta); temp = (1.-alpha) * std::min(sgnorm,delta);
x = temp * wa1 + alpha * x; x = temp * wa1 + alpha * x;
} }