diff --git a/unsupported/Eigen/src/NonLinearOptimization/lmpar.h b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h index 22d1680789..5cb7e40510 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/lmpar.h +++ b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h @@ -199,23 +199,12 @@ void ei_lmpar2( /* compute and store in x the gauss-newton direction. if the */ /* jacobian is rank-deficient, obtain a least squares solution. */ - int nsing = n-1; - wa1 = qtb; - for (j = 0; j < n; ++j) { - if (qr.matrixQR()(j,j) == 0. && nsing == n-1) - nsing = j - 1; - if (nsing < n-1) - wa1[j] = 0.; - } - for (j = nsing; j>=0; --j) { - wa1[j] /= qr.matrixQR()(j,j); - temp = wa1[j]; - for (i = 0; i < j ; ++i) - wa1[i] -= qr.matrixQR()(i,j) * temp; - } +// const int rank = qr.nonzeroPivots(); // exactly double(0.) + const int rank = qr.rank(); // use a threshold + wa1 = qtb; wa1.segment(rank,n-rank).setZero(); + qr.matrixQR().corner(TopLeft, rank, rank).template triangularView().solveInPlace(wa1.head(rank)); - for (j = 0; j < n; ++j) - x[qr.colsPermutation().indices()(j)] = wa1[j]; + x = qr.colsPermutation()*wa1; /* initialize the iteration counter. */ /* evaluate the function at the origin, and test */ @@ -235,19 +224,12 @@ void ei_lmpar2( /* the function. otherwise set this bound to zero. */ parl = 0.; - if (nsing >= n-1) { + if (rank==n) { for (j = 0; j < n; ++j) { l = qr.colsPermutation().indices()(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) { - Scalar sum = 0.; - for (i = 0; i < j; ++i) - sum += qr.matrixQR()(i,j) * wa1[i]; - wa1[j] = (wa1[j] - sum) / qr.matrixQR()(j,j); - } + qr.matrixQR().corner(TopLeft, n, n).transpose().template triangularView().solveInPlace(wa1); temp = wa1.blueNorm(); parl = fp / delta / temp / temp; } @@ -272,7 +254,7 @@ void ei_lmpar2( /* beginning of an iteration. */ - Matrix< Scalar, Dynamic, Dynamic > r = qr.matrixQR(); // TODO : fixme + Matrix< Scalar, Dynamic, Dynamic > s = qr.matrixQR(); while (true) { ++iter; @@ -284,7 +266,7 @@ void ei_lmpar2( wa1 = ei_sqrt(par)* diag; Matrix< Scalar, Dynamic, 1 > sdiag(n); - ei_qrsolv(r, qr.colsPermutation().indices(), wa1, qtb, x, sdiag); + ei_qrsolv(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag); wa2 = diag.cwiseProduct(x); dxnorm = wa2.blueNorm(); @@ -308,7 +290,7 @@ void ei_lmpar2( wa1[j] /= sdiag[j]; temp = wa1[j]; for (i = j+1; i < n; ++i) - wa1[i] -= r(i,j) * temp; + wa1[i] -= s(i,j) * temp; } temp = wa1.blueNorm(); parc = fp / delta / temp / temp; @@ -321,16 +303,8 @@ void ei_lmpar2( paru = std::min(paru,par); /* compute an improved estimate for par. */ - - /* Computing MAX */ par = std::max(parl,par+parc); - - /* end of an iteration. */ - } - - /* termination. */ - if (iter == 0) par = 0.; return; diff --git a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h index 6ffba42c55..880d9d6e39 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h +++ b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h @@ -1,7 +1,8 @@ template void ei_qrsolv( - Matrix< Scalar, Dynamic, Dynamic > &r, + Matrix< Scalar, Dynamic, Dynamic > &s, + // TODO : use a PermutationMatrix once ei_lmpar is no more: const VectorXi &ipvt, const Matrix< Scalar, Dynamic, 1 > &diag, const Matrix< Scalar, Dynamic, 1 > &qtb, @@ -11,21 +12,23 @@ void ei_qrsolv( { /* Local variables */ int i, j, k, l; - Scalar sum, temp; - int n = r.cols(); + Scalar temp; + int n = s.cols(); Matrix< Scalar, Dynamic, 1 > wa(n); /* Function Body */ + // the following will only change the lower triangular part of s, including + // the diagonal, though the diagonal is restored afterward /* copy r and (q transpose)*b to preserve input and initialize s. */ /* in particular, save the diagonal elements of r in x. */ - x = r.diagonal(); + x = s.diagonal(); wa = qtb; for (j = 0; j < n; ++j) for (i = j+1; i < n; ++i) - r(i,j) = r(j,i); + s(i,j) = s(j,i); /* eliminate the diagonal matrix d using a givens rotation. */ for (j = 0; j < n; ++j) { @@ -48,43 +51,37 @@ void ei_qrsolv( /* determine a givens rotation which eliminates the */ /* appropriate element in the current row of d. */ PlanarRotation givens; - givens.makeGivens(-r(k,k), sdiag[k]); + givens.makeGivens(-s(k,k), sdiag[k]); /* compute the modified diagonal element of r and */ /* the modified element of ((q transpose)*b,0). */ - r(k,k) = givens.c() * r(k,k) + givens.s() * sdiag[k]; + s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k]; temp = givens.c() * wa[k] + givens.s() * qtbpj; qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj; wa[k] = temp; /* accumulate the tranformation in the row of s. */ for (i = k+1; i=0; j--) { - sum = 0.; - for (i = j+1; i <= nsing; ++i) - sum += r(i,j) * wa[i]; - wa[j] = (wa[j] - sum) / sdiag[j]; - } + s.corner(TopLeft, nsing, nsing).transpose().template triangularView().solveInPlace(wa.head(nsing)); + + // restore + sdiag = s.diagonal(); + s.diagonal() = x; /* permute the components of z back to components of x. */ for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j]; diff --git a/unsupported/test/NonLinearOptimization.cpp b/unsupported/test/NonLinearOptimization.cpp index baca180521..c1687b8c39 100644 --- a/unsupported/test/NonLinearOptimization.cpp +++ b/unsupported/test/NonLinearOptimization.cpp @@ -1010,7 +1010,7 @@ void testNistLanczos1(void) VERIFY( 79 == lm.nfev); VERIFY( 72 == lm.njev); // check norm^2 - VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.428127827535E-25); // should be 1.4307867721E-25, but nist results are on 128-bit floats + VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.427932429905E-25); // should be 1.4307867721E-25, but nist results are on 128-bit floats // check x VERIFY_IS_APPROX(x[0], 9.5100000027E-02 ); VERIFY_IS_APPROX(x[1], 1.0000000001E+00 ); @@ -1332,8 +1332,8 @@ void testNistMGH17(void) // check return value VERIFY( 2 == info); - VERIFY( 603 == lm.nfev); - VERIFY( 544 == lm.njev); + VERIFY( 606 == lm.nfev); + VERIFY( 545 == lm.njev); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.4648946975E-05); // check x