diff options
author | 2009-08-23 21:39:47 +0200 | |
---|---|---|
committer | 2009-08-23 21:39:47 +0200 | |
commit | 5e8dee7a19391567d3aca478abf09fb1073c48ad (patch) | |
tree | 1e75cd5677956a68251a9761fc7218decbf8e38e /unsupported | |
parent | f793dbe45c1bf82bdb6890fc57fd0a10efd2c17f (diff) |
eigenize dogleg()
Diffstat (limited to 'unsupported')
-rw-r--r-- | unsupported/Eigen/src/NonLinear/dogleg.h | 67 | ||||
-rw-r--r-- | unsupported/Eigen/src/NonLinear/hybrd.h | 2 | ||||
-rw-r--r-- | unsupported/Eigen/src/NonLinear/hybrj.h | 2 |
3 files changed, 33 insertions, 38 deletions
diff --git a/unsupported/Eigen/src/NonLinear/dogleg.h b/unsupported/Eigen/src/NonLinear/dogleg.h index 8afe43c89..b6af54a2e 100644 --- a/unsupported/Eigen/src/NonLinear/dogleg.h +++ b/unsupported/Eigen/src/NonLinear/dogleg.h @@ -1,8 +1,11 @@ - template <typename Scalar> -void ei_dogleg(int n, const Scalar *r__, int /* lr*/ , - const Scalar *diag, const Scalar *qtb, Scalar delta, Scalar *x, - Scalar *wa1, Scalar *wa2) +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, jp1; @@ -10,30 +13,26 @@ void ei_dogleg(int n, const Scalar *r__, int /* lr*/ , Scalar gnorm, qnorm; Scalar sgnorm; - /* Parameter adjustments */ - --wa2; - --wa1; - --x; - --qtb; - --diag; - --r__; - /* 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 + 1; - for (k = 1; k <= n; ++k) { - j = n - k + 1; + jj = n * (n + 1) / 2; + for (k = 0; k < n; ++k) { + j = n - k - 1; jp1 = j + 1; - jj -= k; + jj -= k+1; l = jj + 1; sum = 0.; if (n < jp1) { goto L20; } - for (i = jp1; i <= n; ++i) { + for (i = jp1; i < n; ++i) { sum += r__[l] * x[i]; ++l; /* L10: */ @@ -44,7 +43,7 @@ L20: goto L40; } l = j; - for (i = 1; i <= j; ++i) { + for (i = 0; i <= j; ++i) { /* Computing MAX */ temp = std::max(temp,ei_abs(r__[l])); l = l + n - i; @@ -61,12 +60,12 @@ L40: /* test whether the gauss-newton direction is acceptable. */ - for (j = 1; j <= n; ++j) { + for (j = 0; j < n; ++j) { wa1[j] = 0.; wa2[j] = diag[j] * x[j]; /* L60: */ } - qnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&wa2[1],n).stableNorm(); + qnorm = wa2.stableNorm(); if (qnorm <= delta) { /* goto L140; */ return; @@ -75,10 +74,10 @@ L40: /* the gauss-newton direction is not acceptable. */ /* next, calculate the scaled gradient direction. */ - l = 1; - for (j = 1; j <= n; ++j) { + l = 0; + for (j = 0; j < n; ++j) { temp = qtb[j]; - for (i = j; i <= n; ++i) { + for (i = j; i < n; ++i) { wa1[i] += r__[l] * temp; ++l; /* L70: */ @@ -90,7 +89,7 @@ L40: /* calculate the norm of the scaled gradient and test for */ /* the special case in which the scaled gradient is zero. */ - gnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&wa1[1],n).stableNorm(); + gnorm = wa1.stableNorm(); sgnorm = 0.; alpha = delta / qnorm; if (gnorm == 0.) { @@ -100,14 +99,14 @@ L40: /* calculate the point along the scaled gradient */ /* at which the quadratic is minimized. */ - for (j = 1; j <= n; ++j) { + for (j = 0; j < n; ++j) { wa1[j] = wa1[j] / gnorm / diag[j]; /* L90: */ } - l = 1; - for (j = 1; j <= n; ++j) { + l = 0; + for (j = 0; j < n; ++j) { sum = 0.; - for (i = j; i <= n; ++i) { + for (i = j; i < n; ++i) { sum += r__[l] * wa1[i]; ++l; /* L100: */ @@ -115,7 +114,7 @@ L40: wa2[j] = sum; /* L110: */ } - temp = Map< Matrix< Scalar, Dynamic, 1 > >(&wa2[1],n).stableNorm(); + temp = wa2.stableNorm(); sgnorm = gnorm / temp / temp; /* test whether the scaled gradient direction is acceptable. */ @@ -129,7 +128,7 @@ L40: /* finally, calculate the point along the dogleg */ /* at which the quadratic is minimized. */ - bnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&qtb[1],n).stableNorm(); + 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))); @@ -141,14 +140,10 @@ L120: /* direction and the scaled gradient direction. */ temp = (1. - alpha) * std::min(sgnorm,delta); - for (j = 1; j <= n; ++j) { + for (j = 0; j < n; ++j) { x[j] = temp * wa1[j] + alpha * x[j]; - /* L130: */ } - /* L140: */ return; - /* last card of subroutine dogleg. */ - -} /* dogleg_ */ +} diff --git a/unsupported/Eigen/src/NonLinear/hybrd.h b/unsupported/Eigen/src/NonLinear/hybrd.h index 3b17e444b..987373cb6 100644 --- a/unsupported/Eigen/src/NonLinear/hybrd.h +++ b/unsupported/Eigen/src/NonLinear/hybrd.h @@ -209,7 +209,7 @@ L190: /* determine the direction p. */ - ei_dogleg<Scalar>(n, R.data(), lr, diag.data(), qtf.data(), delta, wa1.data(), wa2.data(), wa3.data()); + ei_dogleg<Scalar>(R, diag, qtf, delta, wa1); /* store the direction p and x + p. calculate the norm of p. */ diff --git a/unsupported/Eigen/src/NonLinear/hybrj.h b/unsupported/Eigen/src/NonLinear/hybrj.h index 78c8a00a8..ef4d88c60 100644 --- a/unsupported/Eigen/src/NonLinear/hybrj.h +++ b/unsupported/Eigen/src/NonLinear/hybrj.h @@ -198,7 +198,7 @@ L190: /* determine the direction p. */ - ei_dogleg<Scalar>(n, R.data(), lr, diag.data(), qtf.data(), delta, wa1.data(), wa2.data(), wa3.data()); + ei_dogleg<Scalar>(R, diag, qtf, delta, wa1); /* store the direction p and x + p. calculate the norm of p. */ |