mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-08-12 11:49:02 +08:00
further cleaning/ goto removing
This commit is contained in:
Thomas Capricelli
parent
92a5bb4539
commit
312ab1abb3
@ -20,7 +20,7 @@ void ei_dogleg(
|
||||
assert(n==qtb.size());
|
||||
assert(n==x.size());
|
||||
|
||||
/* first, calculate the gauss-newton direction. */
|
||||
/* first, calculate the gauss-newton direction. */
|
||||
|
||||
jj = n * (n + 1) / 2;
|
||||
for (k = 0; k < n; ++k) {
|
||||
@ -39,17 +39,15 @@ void ei_dogleg(
|
||||
/* Computing MAX */
|
||||
temp = std::max(temp,ei_abs(r[l]));
|
||||
l = l + n - i;
|
||||
/* L30: */
|
||||
}
|
||||
temp = epsmch * temp;
|
||||
if (temp == 0.) {
|
||||
if (temp == 0.)
|
||||
temp = epsmch;
|
||||
}
|
||||
}
|
||||
x[j] = (qtb[j] - sum) / temp;
|
||||
}
|
||||
|
||||
/* test whether the gauss-newton direction is acceptable. */
|
||||
/* test whether the gauss-newton direction is acceptable. */
|
||||
|
||||
wa1.fill(0.);
|
||||
wa2 = diag.cwise() * x;
|
||||
@ -57,8 +55,8 @@ void ei_dogleg(
|
||||
if (qnorm <= delta)
|
||||
return;
|
||||
|
||||
/* the gauss-newton direction is not acceptable. */
|
||||
/* next, calculate the scaled gradient direction. */
|
||||
/* the gauss-newton direction is not acceptable. */
|
||||
/* next, calculate the scaled gradient direction. */
|
||||
|
||||
l = 0;
|
||||
for (j = 0; j < n; ++j) {
|
||||
@ -70,17 +68,17 @@ void ei_dogleg(
|
||||
wa1[j] /= diag[j];
|
||||
}
|
||||
|
||||
/* calculate the norm of the scaled gradient and test for */
|
||||
/* the special case in which the scaled gradient is zero. */
|
||||
/* 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 L120;
|
||||
goto algo_end;
|
||||
|
||||
/* calculate the point along the scaled gradient */
|
||||
/* at which the quadratic is minimized. */
|
||||
/* calculate the point along the scaled gradient */
|
||||
/* at which the quadratic is minimized. */
|
||||
|
||||
wa1.cwise() /= diag*gnorm;
|
||||
l = 0;
|
||||
@ -97,16 +95,15 @@ void ei_dogleg(
|
||||
temp = wa2.stableNorm();
|
||||
sgnorm = gnorm / temp / temp;
|
||||
|
||||
/* test whether the scaled gradient direction is acceptable. */
|
||||
/* test whether the scaled gradient direction is acceptable. */
|
||||
|
||||
alpha = 0.;
|
||||
if (sgnorm >= delta) {
|
||||
goto L120;
|
||||
}
|
||||
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. */
|
||||
/* 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);
|
||||
@ -114,10 +111,10 @@ void ei_dogleg(
|
||||
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;
|
||||
L120:
|
||||
algo_end:
|
||||
|
||||
/* form appropriate convex combination of the gauss-newton */
|
||||
/* direction and the scaled gradient direction. */
|
||||
/* 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;
|
||||
|
||||
@ -30,8 +30,8 @@ void ei_lmpar(
|
||||
|
||||
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. */
|
||||
/* 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) {
|
||||
@ -54,39 +54,40 @@ void ei_lmpar(
|
||||
x[l] = wa1[j];
|
||||
}
|
||||
|
||||
/* initialize the iteration counter. */
|
||||
/* evaluate the function at the origin, and test */
|
||||
/* for acceptance of the gauss-newton direction. */
|
||||
/* 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)
|
||||
goto L220;
|
||||
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. */
|
||||
/* 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)
|
||||
goto L120;
|
||||
for (j = 0; j < n; ++j) {
|
||||
l = ipvt[j];
|
||||
wa1[j] = diag[l] * (wa2[l] / dxnorm);
|
||||
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;
|
||||
}
|
||||
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;
|
||||
L120:
|
||||
|
||||
/* calculate an upper bound, paru, for the zero of the function. */
|
||||
/* calculate an upper bound, paru, for the zero of the function. */
|
||||
|
||||
for (j = 0; j < n; ++j) {
|
||||
sum = 0.;
|
||||
@ -97,89 +98,82 @@ L120:
|
||||
}
|
||||
gnorm = wa1.stableNorm();
|
||||
paru = gnorm / delta;
|
||||
if (paru == 0.) {
|
||||
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. */
|
||||
/* 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. */
|
||||
/* beginning of an iteration. */
|
||||
|
||||
L150:
|
||||
++iter;
|
||||
while (true) {
|
||||
++iter;
|
||||
|
||||
/* evaluate the function at the current value of par. */
|
||||
/* evaluate the function at the current value of par. */
|
||||
|
||||
if (par == 0.)
|
||||
par = std::max(dwarf,Scalar(.001) * paru); /* Computing MAX */
|
||||
if (par == 0.)
|
||||
par = std::max(dwarf,Scalar(.001) * paru); /* Computing MAX */
|
||||
|
||||
temp = ei_sqrt(par);
|
||||
wa1 = temp * diag;
|
||||
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;
|
||||
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;
|
||||
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 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. */
|
||||
|
||||
if (ei_abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) ||
|
||||
iter == 10) {
|
||||
goto L220;
|
||||
}
|
||||
|
||||
/* compute the newton correction. */
|
||||
/* termination. */
|
||||
|
||||
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. */
|
||||
|
||||
goto L150;
|
||||
L220:
|
||||
|
||||
/* termination. */
|
||||
|
||||
if (iter == 0) {
|
||||
if (iter == 0)
|
||||
par = 0.;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user