diff options
| -rw-r--r-- | unsupported/Eigen/src/NonLinearOptimization/lmpar.h | 38 | ||||
| -rw-r--r-- | unsupported/Eigen/src/NonLinearOptimization/qrsolv.h | 10 |
2 files changed, 16 insertions, 32 deletions
diff --git a/unsupported/Eigen/src/NonLinearOptimization/lmpar.h b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h index 77c07a8db..b723a7e0a 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/lmpar.h +++ b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h @@ -10,12 +10,11 @@ void ei_lmpar( Matrix< Scalar, Dynamic, 1 > &x) { /* Local variables */ - int i, j, k, l; + int i, j, l; Scalar fp; - Scalar sum, parc, parl; + Scalar parc, parl; int iter; Scalar temp, paru; - int nsing; Scalar gnorm; Scalar dxnorm; @@ -27,31 +26,28 @@ void ei_lmpar( assert(n==qtb.size()); assert(n==x.size()); - Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n); + Matrix< Scalar, Dynamic, 1 > wa1, wa2; /* compute and store in x the gauss-newton direction. if the */ /* jacobian is rank-deficient, obtain a least squares solution. */ - nsing = n-1; + int nsing = n-1; + wa1 = qtb; 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; + for (j = nsing; j>=0; --j) { 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]; - } + for (j = 0; j < n; ++j) + x[ipvt[j]] = wa1[j]; /* initialize the iteration counter. */ /* evaluate the function at the origin, and test */ @@ -76,8 +72,10 @@ void ei_lmpar( l = ipvt[j]; wa1[j] = diag[l] * (wa2[l] / dxnorm); } + // it's actually a triangularView.solveInplace(), though in a weird + // way: for (j = 0; j < n; ++j) { - sum = 0.; + Scalar sum = 0.; for (i = 0; i < j; ++i) sum += r(i,j) * wa1[i]; wa1[j] = (wa1[j] - sum) / r(j,j); @@ -88,13 +86,9 @@ void ei_lmpar( /* 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]; - } + for (j = 0; j < n; ++j) + wa1[j] = r.col(j).start(j+1).dot(qtb.start(j+1)) / diag[ipvt[j]]; + gnorm = wa1.stableNorm(); paru = gnorm / delta; if (paru == 0.) @@ -118,8 +112,7 @@ void ei_lmpar( if (par == 0.) par = std::max(dwarf,Scalar(.001) * paru); /* Computing MAX */ - temp = ei_sqrt(par); - wa1 = temp * diag; + wa1 = ei_sqrt(par)* diag; Matrix< Scalar, Dynamic, 1 > sdiag(n); ei_qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag); @@ -141,7 +134,6 @@ void ei_lmpar( 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]; diff --git a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h index db44585d1..1ee21d5ed 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h +++ b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h @@ -1,11 +1,4 @@ -#if 0 - int n, Scalar *r__, int ldr, - const int *ipvt, const Scalar *diag, const Scalar *qtb, Scalar *x, - Scalar *sdiag) -#endif - - template <typename Scalar> void ei_qrsolv( Matrix< Scalar, Dynamic, Dynamic > &r, @@ -19,7 +12,6 @@ void ei_qrsolv( /* Local variables */ int i, j, k, l; Scalar sum, temp; - Scalar qtbpj; int n = r.cols(); Matrix< Scalar, Dynamic, 1 > wa(n); @@ -51,7 +43,7 @@ void ei_qrsolv( /* modify only a single element of (q transpose)*b */ /* beyond the first n, which is initially zero. */ - qtbpj = 0.; + Scalar qtbpj = 0.; for (k = j; k < n; ++k) { /* determine a givens rotation which eliminates the */ /* appropriate element in the current row of d. */ |