mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-06-04 18:54:00 +08:00
only indentation fixes (this eases porting)
This commit is contained in:
Thomas Capricelli
parent
feb5af3ede
commit
f793dbe45c
@ -1,8 +1,8 @@
|
|||||||
|
|
||||||
template <typename Scalar>
|
template <typename Scalar>
|
||||||
void ei_dogleg(int n, const Scalar *r__, int /* lr*/ ,
|
void ei_dogleg(int n, const Scalar *r__, int /* lr*/ ,
|
||||||
const Scalar *diag, const Scalar *qtb, Scalar delta, Scalar *x,
|
const Scalar *diag, const Scalar *qtb, Scalar delta, Scalar *x,
|
||||||
Scalar *wa1, Scalar *wa2)
|
Scalar *wa1, Scalar *wa2)
|
||||||
{
|
{
|
||||||
/* Local variables */
|
/* Local variables */
|
||||||
int i, j, k, l, jj, jp1;
|
int i, j, k, l, jj, jp1;
|
||||||
@ -21,134 +21,134 @@ void ei_dogleg(int n, const Scalar *r__, int /* lr*/ ,
|
|||||||
/* Function Body */
|
/* Function Body */
|
||||||
const Scalar epsmch = epsilon<Scalar>();
|
const Scalar epsmch = epsilon<Scalar>();
|
||||||
|
|
||||||
/* first, calculate the gauss-newton direction. */
|
/* first, calculate the gauss-newton direction. */
|
||||||
|
|
||||||
jj = n * (n + 1) / 2 + 1;
|
jj = n * (n + 1) / 2 + 1;
|
||||||
for (k = 1; k <= n; ++k) {
|
for (k = 1; k <= n; ++k) {
|
||||||
j = n - k + 1;
|
j = n - k + 1;
|
||||||
jp1 = j + 1;
|
jp1 = j + 1;
|
||||||
jj -= k;
|
jj -= k;
|
||||||
l = jj + 1;
|
l = jj + 1;
|
||||||
sum = 0.;
|
sum = 0.;
|
||||||
if (n < jp1) {
|
if (n < jp1) {
|
||||||
goto L20;
|
goto L20;
|
||||||
}
|
}
|
||||||
for (i = jp1; i <= n; ++i) {
|
for (i = jp1; i <= n; ++i) {
|
||||||
sum += r__[l] * x[i];
|
sum += r__[l] * x[i];
|
||||||
++l;
|
++l;
|
||||||
/* L10: */
|
/* L10: */
|
||||||
}
|
}
|
||||||
L20:
|
L20:
|
||||||
temp = r__[jj];
|
temp = r__[jj];
|
||||||
if (temp != 0.) {
|
if (temp != 0.) {
|
||||||
goto L40;
|
goto L40;
|
||||||
}
|
}
|
||||||
l = j;
|
l = j;
|
||||||
for (i = 1; i <= j; ++i) {
|
for (i = 1; i <= j; ++i) {
|
||||||
/* Computing MAX */
|
/* Computing MAX */
|
||||||
temp = std::max(temp,ei_abs(r__[l]));
|
temp = std::max(temp,ei_abs(r__[l]));
|
||||||
l = l + n - i;
|
l = l + n - i;
|
||||||
/* L30: */
|
/* L30: */
|
||||||
}
|
}
|
||||||
temp = epsmch * temp;
|
temp = epsmch * temp;
|
||||||
if (temp == 0.) {
|
if (temp == 0.) {
|
||||||
temp = epsmch;
|
temp = epsmch;
|
||||||
}
|
}
|
||||||
L40:
|
L40:
|
||||||
x[j] = (qtb[j] - sum) / temp;
|
x[j] = (qtb[j] - sum) / temp;
|
||||||
/* L50: */
|
/* L50: */
|
||||||
}
|
}
|
||||||
|
|
||||||
/* test whether the gauss-newton direction is acceptable. */
|
/* test whether the gauss-newton direction is acceptable. */
|
||||||
|
|
||||||
for (j = 1; j <= n; ++j) {
|
for (j = 1; j <= n; ++j) {
|
||||||
wa1[j] = 0.;
|
wa1[j] = 0.;
|
||||||
wa2[j] = diag[j] * x[j];
|
wa2[j] = diag[j] * x[j];
|
||||||
/* L60: */
|
/* L60: */
|
||||||
}
|
}
|
||||||
qnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&wa2[1],n).stableNorm();
|
qnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&wa2[1],n).stableNorm();
|
||||||
if (qnorm <= delta) {
|
if (qnorm <= delta) {
|
||||||
/* goto L140; */
|
/* goto L140; */
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* 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. */
|
||||||
|
|
||||||
l = 1;
|
l = 1;
|
||||||
for (j = 1; j <= n; ++j) {
|
for (j = 1; j <= n; ++j) {
|
||||||
temp = qtb[j];
|
temp = qtb[j];
|
||||||
for (i = j; i <= n; ++i) {
|
for (i = j; i <= n; ++i) {
|
||||||
wa1[i] += r__[l] * temp;
|
wa1[i] += r__[l] * temp;
|
||||||
++l;
|
++l;
|
||||||
/* L70: */
|
/* L70: */
|
||||||
}
|
}
|
||||||
wa1[j] /= diag[j];
|
wa1[j] /= diag[j];
|
||||||
/* L80: */
|
/* L80: */
|
||||||
}
|
}
|
||||||
|
|
||||||
/* 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 = Map< Matrix< Scalar, Dynamic, 1 > >(&wa1[1],n).stableNorm();
|
gnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&wa1[1],n).stableNorm();
|
||||||
sgnorm = 0.;
|
sgnorm = 0.;
|
||||||
alpha = delta / qnorm;
|
alpha = delta / qnorm;
|
||||||
if (gnorm == 0.) {
|
if (gnorm == 0.) {
|
||||||
goto L120;
|
goto L120;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* 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. */
|
||||||
|
|
||||||
for (j = 1; j <= n; ++j) {
|
for (j = 1; j <= n; ++j) {
|
||||||
wa1[j] = wa1[j] / gnorm / diag[j];
|
wa1[j] = wa1[j] / gnorm / diag[j];
|
||||||
/* L90: */
|
/* L90: */
|
||||||
}
|
}
|
||||||
l = 1;
|
l = 1;
|
||||||
for (j = 1; j <= n; ++j) {
|
for (j = 1; j <= n; ++j) {
|
||||||
sum = 0.;
|
sum = 0.;
|
||||||
for (i = j; i <= n; ++i) {
|
for (i = j; i <= n; ++i) {
|
||||||
sum += r__[l] * wa1[i];
|
sum += r__[l] * wa1[i];
|
||||||
++l;
|
++l;
|
||||||
/* L100: */
|
/* L100: */
|
||||||
}
|
}
|
||||||
wa2[j] = sum;
|
wa2[j] = sum;
|
||||||
/* L110: */
|
/* L110: */
|
||||||
}
|
}
|
||||||
temp = Map< Matrix< Scalar, Dynamic, 1 > >(&wa2[1],n).stableNorm();
|
temp = Map< Matrix< Scalar, Dynamic, 1 > >(&wa2[1],n).stableNorm();
|
||||||
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 L120;
|
goto L120;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* 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 = Map< Matrix< Scalar, Dynamic, 1 > >(&qtb[1],n).stableNorm();
|
bnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&qtb[1],n).stableNorm();
|
||||||
temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
|
temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
|
||||||
/* Computing 2nd power */
|
/* 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 */
|
/* Computing 2nd power */
|
||||||
alpha = delta / qnorm * (1. - ei_abs2(sgnorm / delta)) / temp;
|
alpha = delta / qnorm * (1. - ei_abs2(sgnorm / delta)) / temp;
|
||||||
L120:
|
L120:
|
||||||
|
|
||||||
/* 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);
|
||||||
for (j = 1; j <= n; ++j) {
|
for (j = 1; j <= n; ++j) {
|
||||||
x[j] = temp * wa1[j] + alpha * x[j];
|
x[j] = temp * wa1[j] + alpha * x[j];
|
||||||
/* L130: */
|
/* L130: */
|
||||||
}
|
}
|
||||||
/* L140: */
|
/* L140: */
|
||||||
return;
|
return;
|
||||||
|
|
||||||
/* last card of subroutine dogleg. */
|
/* last card of subroutine dogleg. */
|
||||||
|
|
||||||
} /* dogleg_ */
|
} /* dogleg_ */
|
||||||
|
|
||||||
|
|||||||
@ -31,174 +31,174 @@ void ei_lmpar(
|
|||||||
|
|
||||||
Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n);
|
Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n);
|
||||||
|
|
||||||
/* compute and store in x the gauss-newton direction. if the */
|
/* compute and store in x the gauss-newton direction. if the */
|
||||||
/* jacobian is rank-deficient, obtain a least squares solution. */
|
/* jacobian is rank-deficient, obtain a least squares solution. */
|
||||||
|
|
||||||
nsing = n-1;
|
nsing = n-1;
|
||||||
for (j = 0; j < n; ++j) {
|
for (j = 0; j < n; ++j) {
|
||||||
wa1[j] = qtb[j];
|
wa1[j] = qtb[j];
|
||||||
if (r__(j,j) == 0. && nsing == n-1)
|
if (r__(j,j) == 0. && nsing == n-1)
|
||||||
nsing = j - 1;
|
nsing = j - 1;
|
||||||
if (nsing < n-1)
|
if (nsing < n-1)
|
||||||
wa1[j] = 0.;
|
wa1[j] = 0.;
|
||||||
}
|
}
|
||||||
for (k = 0; k <= nsing; ++k) {
|
for (k = 0; k <= nsing; ++k) {
|
||||||
j = nsing - k;
|
j = nsing - k;
|
||||||
wa1[j] /= r__(j,j);
|
wa1[j] /= r__(j,j);
|
||||||
temp = wa1[j];
|
temp = wa1[j];
|
||||||
jm1 = j - 1;
|
jm1 = j - 1;
|
||||||
for (i = 0; i <= jm1; ++i)
|
for (i = 0; i <= jm1; ++i)
|
||||||
wa1[i] -= r__(i,j) * temp;
|
wa1[i] -= r__(i,j) * temp;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (j = 0; j < n; ++j) {
|
for (j = 0; j < n; ++j) {
|
||||||
l = ipvt[j]-1;
|
l = ipvt[j]-1;
|
||||||
x[l] = wa1[j];
|
x[l] = wa1[j];
|
||||||
}
|
}
|
||||||
|
|
||||||
/* initialize the iteration counter. */
|
/* initialize the iteration counter. */
|
||||||
/* evaluate the function at the origin, and test */
|
/* evaluate the function at the origin, and test */
|
||||||
/* for acceptance of the gauss-newton direction. */
|
/* for acceptance of the gauss-newton direction. */
|
||||||
|
|
||||||
iter = 0;
|
iter = 0;
|
||||||
for (j = 0; j < n; ++j) {
|
for (j = 0; j < n; ++j) {
|
||||||
wa2[j] = diag[j] * x[j];
|
wa2[j] = diag[j] * x[j];
|
||||||
/* L70: */
|
/* L70: */
|
||||||
}
|
}
|
||||||
dxnorm = wa2.blueNorm();
|
dxnorm = wa2.blueNorm();
|
||||||
fp = dxnorm - delta;
|
fp = dxnorm - delta;
|
||||||
if (fp <= Scalar(0.1) * delta) {
|
if (fp <= Scalar(0.1) * delta) {
|
||||||
goto L220;
|
goto L220;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* if the jacobian is not rank deficient, the newton */
|
/* if the jacobian is not rank deficient, the newton */
|
||||||
/* step provides a lower bound, parl, for the zero of */
|
/* step provides a lower bound, parl, for the zero of */
|
||||||
/* the function. otherwise set this bound to zero. */
|
/* the function. otherwise set this bound to zero. */
|
||||||
|
|
||||||
parl = 0.;
|
parl = 0.;
|
||||||
if (nsing < n-1) {
|
if (nsing < n-1) {
|
||||||
goto L120;
|
goto L120;
|
||||||
}
|
}
|
||||||
for (j = 0; j < n; ++j) {
|
for (j = 0; j < n; ++j) {
|
||||||
l = ipvt[j]-1;
|
l = ipvt[j]-1;
|
||||||
wa1[j] = diag[l] * (wa2[l] / dxnorm);
|
wa1[j] = diag[l] * (wa2[l] / dxnorm);
|
||||||
}
|
}
|
||||||
for (j = 0; j < n; ++j) {
|
for (j = 0; j < n; ++j) {
|
||||||
sum = 0.;
|
sum = 0.;
|
||||||
jm1 = j - 1;
|
jm1 = j - 1;
|
||||||
for (i = 0; i <= jm1; ++i)
|
for (i = 0; i <= jm1; ++i)
|
||||||
sum += r__(i,j) * wa1[i];
|
sum += r__(i,j) * wa1[i];
|
||||||
wa1[j] = (wa1[j] - sum) / r__(j,j);
|
wa1[j] = (wa1[j] - sum) / r__(j,j);
|
||||||
}
|
}
|
||||||
temp = wa1.blueNorm();
|
temp = wa1.blueNorm();
|
||||||
parl = fp / delta / temp / temp;
|
parl = fp / delta / temp / temp;
|
||||||
L120:
|
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) {
|
for (j = 0; j < n; ++j) {
|
||||||
sum = 0.;
|
sum = 0.;
|
||||||
for (i = 0; i <= j; ++i) {
|
for (i = 0; i <= j; ++i) {
|
||||||
sum += r__(i,j) * qtb[i];
|
sum += r__(i,j) * qtb[i];
|
||||||
/* L130: */
|
/* L130: */
|
||||||
}
|
}
|
||||||
l = ipvt[j]-1;
|
l = ipvt[j]-1;
|
||||||
wa1[j] = sum / diag[l];
|
wa1[j] = sum / diag[l];
|
||||||
/* L140: */
|
/* L140: */
|
||||||
}
|
}
|
||||||
gnorm = wa1.stableNorm();
|
gnorm = wa1.stableNorm();
|
||||||
paru = gnorm / delta;
|
paru = gnorm / delta;
|
||||||
if (paru == 0.) {
|
if (paru == 0.) {
|
||||||
paru = dwarf / std::min(delta,Scalar(0.1));
|
paru = dwarf / std::min(delta,Scalar(0.1));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* if the input par lies outside of the interval (parl,paru), */
|
/* if the input par lies outside of the interval (parl,paru), */
|
||||||
/* set par to the closer endpoint. */
|
/* set par to the closer endpoint. */
|
||||||
|
|
||||||
par = std::max(par,parl);
|
par = std::max(par,parl);
|
||||||
par = std::min(par,paru);
|
par = std::min(par,paru);
|
||||||
if (par == 0.) {
|
if (par == 0.) {
|
||||||
par = gnorm / dxnorm;
|
par = gnorm / dxnorm;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* beginning of an iteration. */
|
/* beginning of an iteration. */
|
||||||
|
|
||||||
L150:
|
L150:
|
||||||
++iter;
|
++iter;
|
||||||
|
|
||||||
/* evaluate the function at the current value of par. */
|
/* evaluate the function at the current value of par. */
|
||||||
|
|
||||||
if (par == 0.) {
|
if (par == 0.) {
|
||||||
/* Computing MAX */
|
/* Computing MAX */
|
||||||
par = std::max(dwarf,Scalar(.001) * paru);
|
par = std::max(dwarf,Scalar(.001) * paru);
|
||||||
}
|
}
|
||||||
temp = ei_sqrt(par);
|
temp = ei_sqrt(par);
|
||||||
for (j = 0; j < n; ++j) {
|
for (j = 0; j < n; ++j) {
|
||||||
wa1[j] = temp * diag[j];
|
wa1[j] = temp * diag[j];
|
||||||
/* L160: */
|
/* L160: */
|
||||||
}
|
}
|
||||||
ei_qrsolv<Scalar>(n, r__.data(), r__.rows(), ipvt.data(), wa1.data(), qtb.data(), x.data(), sdiag.data(), wa2.data());
|
ei_qrsolv<Scalar>(n, r__.data(), r__.rows(), ipvt.data(), wa1.data(), qtb.data(), x.data(), sdiag.data(), wa2.data());
|
||||||
for (j = 0; j < n; ++j) {
|
for (j = 0; j < n; ++j) {
|
||||||
wa2[j] = diag[j] * x[j];
|
wa2[j] = diag[j] * x[j];
|
||||||
/* L170: */
|
/* L170: */
|
||||||
}
|
}
|
||||||
dxnorm = wa2.blueNorm();
|
dxnorm = wa2.blueNorm();
|
||||||
temp = fp;
|
temp = fp;
|
||||||
fp = dxnorm - delta;
|
fp = dxnorm - delta;
|
||||||
|
|
||||||
/* if the function is small enough, accept the current value */
|
/* if the function is small enough, accept the current value */
|
||||||
/* of par. also test for the exceptional cases where parl */
|
/* of par. also test for the exceptional cases where parl */
|
||||||
/* is zero or the number of iterations has reached 10. */
|
/* is zero or the number of iterations has reached 10. */
|
||||||
|
|
||||||
if (ei_abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) ||
|
if (ei_abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) ||
|
||||||
iter == 10) {
|
iter == 10) {
|
||||||
goto L220;
|
goto L220;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* compute the newton correction. */
|
/* compute the newton correction. */
|
||||||
|
|
||||||
for (j = 0; j < n; ++j) {
|
for (j = 0; j < n; ++j) {
|
||||||
l = ipvt[j]-1;
|
l = ipvt[j]-1;
|
||||||
wa1[j] = diag[l] * (wa2[l] / dxnorm);
|
wa1[j] = diag[l] * (wa2[l] / dxnorm);
|
||||||
/* L180: */
|
/* L180: */
|
||||||
}
|
}
|
||||||
for (j = 0; j < n; ++j) {
|
for (j = 0; j < n; ++j) {
|
||||||
wa1[j] /= sdiag[j];
|
wa1[j] /= sdiag[j];
|
||||||
temp = wa1[j];
|
temp = wa1[j];
|
||||||
jp1 = j + 1;
|
jp1 = j + 1;
|
||||||
for (i = jp1; i < n; ++i)
|
for (i = jp1; i < n; ++i)
|
||||||
wa1[i] -= r__(i,j) * temp;
|
wa1[i] -= r__(i,j) * temp;
|
||||||
}
|
}
|
||||||
temp = wa1.blueNorm();
|
temp = wa1.blueNorm();
|
||||||
parc = fp / delta / temp / temp;
|
parc = fp / delta / temp / temp;
|
||||||
|
|
||||||
/* depending on the sign of the function, update parl or paru. */
|
/* depending on the sign of the function, update parl or paru. */
|
||||||
|
|
||||||
if (fp > 0.) {
|
if (fp > 0.) {
|
||||||
parl = std::max(parl,par);
|
parl = std::max(parl,par);
|
||||||
}
|
}
|
||||||
if (fp < 0.) {
|
if (fp < 0.) {
|
||||||
paru = std::min(paru,par);
|
paru = std::min(paru,par);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* compute an improved estimate for par. */
|
/* compute an improved estimate for par. */
|
||||||
|
|
||||||
/* Computing MAX */
|
/* Computing MAX */
|
||||||
par = std::max(parl,par+parc);
|
par = std::max(parl,par+parc);
|
||||||
|
|
||||||
/* end of an iteration. */
|
/* end of an iteration. */
|
||||||
|
|
||||||
goto L150;
|
goto L150;
|
||||||
L220:
|
L220:
|
||||||
|
|
||||||
/* termination. */
|
/* termination. */
|
||||||
|
|
||||||
if (iter == 0) {
|
if (iter == 0) {
|
||||||
par = 0.;
|
par = 0.;
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
|
|
||||||
/* last card of subroutine lmpar. */
|
/* last card of subroutine lmpar. */
|
||||||
|
|
||||||
} /* lmpar_ */
|
} /* lmpar_ */
|
||||||
|
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user