diff options
5 files changed, 71 insertions, 134 deletions
diff --git a/unsupported/Eigen/NonLinearOptimization b/unsupported/Eigen/NonLinearOptimization index f15e09386..a239018b0 100644 --- a/unsupported/Eigen/NonLinearOptimization +++ b/unsupported/Eigen/NonLinearOptimization @@ -129,7 +129,6 @@ namespace Eigen { #include "src/NonLinearOptimization/r1updt.h" #include "src/NonLinearOptimization/r1mpyq.h" #include "src/NonLinearOptimization/rwupdt.h" -#include "src/NonLinearOptimization/qrfac.h" #include "src/NonLinearOptimization/fdjac1.h" #include "src/NonLinearOptimization/qform.h" #include "src/NonLinearOptimization/lmpar.h" diff --git a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h index 3bf3d12ea..91efdac68 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h +++ b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h @@ -218,7 +218,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep( const int mode ) { - int i, j, l, iwa[1]; + int i, j, l; jeval = true; /* calculate the jacobian matrix. */ @@ -249,7 +249,13 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep( } /* compute the qr factorization of the jacobian. */ - ei_qrfac<Scalar>(n, n, fjac.data(), fjac.rows(), false, iwa, wa1.data()); + wa2 = fjac.colwise().blueNorm(); + HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: + fjac = qrfac.matrixQR(); + wa1 = fjac.diagonal(); + fjac.diagonal() = qrfac.hCoeffs(); + // TODO : avoid this: + for(int ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors /* form (q transpose)*fvec and store in qtf. */ @@ -280,6 +286,11 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep( /* accumulate the orthogonal factor in fjac. */ ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data()); +#if 0 + std::cout << "ei_qform<Scalar>: " << fjac << std::endl; + fjac = qrfac.matrixQ(); + std::cout << "qrfac.matrixQ():" << fjac << std::endl; +#endif /* rescale if necessary. */ @@ -530,7 +541,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep( const int mode ) { - int i, j, l, iwa[1]; + int i, j, l; jeval = true; if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1; if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1; @@ -563,7 +574,13 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep( } /* compute the qr factorization of the jacobian. */ - ei_qrfac<Scalar>(n, n, fjac.data(), fjac.rows(), false, iwa, wa1.data()); + wa2 = fjac.colwise().blueNorm(); + HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: + fjac = qrfac.matrixQR(); + wa1 = fjac.diagonal(); + fjac.diagonal() = qrfac.hCoeffs(); + // TODO : avoid this: + for(int ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors /* form (q transpose)*fvec and store in qtf. */ @@ -594,6 +611,11 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep( /* accumulate the orthogonal factor in fjac. */ ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data()); +#if 0 + std::cout << "ei_qform<Scalar>: " << fjac << std::endl; + fjac = qrfac.matrixQ(); + std::cout << "qrfac.matrixQ():" << fjac << std::endl; +#endif /* rescale if necessary. */ diff --git a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h index 8df48d2ab..9f3707952 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h +++ b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h @@ -254,8 +254,13 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep( /* compute the qr factorization of the jacobian. */ wa2 = fjac.colwise().blueNorm(); - ei_qrfac<Scalar>(m, n, fjac.data(), fjac.rows(), true, ipvt.data(), wa1.data()); - ipvt.array() -= 1; // qrfac() creates ipvt with fortran convention (1->n), convert it to c (0->n-1) + ColPivHouseholderQR<JacobianType> qrfac(fjac); + fjac = qrfac.matrixQR(); + wa1 = fjac.diagonal(); + fjac.diagonal() = qrfac.hCoeffs(); + ipvt = qrfac.colsPermutation().indices(); + // TODO : avoid this: + for(int i=0; i< fjac.cols(); i++) fjac.col(i).segment(i+1, fjac.rows()-i-1) *= fjac(i,i); // rescale vectors /* on the first iteration and if mode is 1, scale according */ /* to the norms of the columns of the initial jacobian. */ @@ -295,6 +300,12 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep( qtf[j] = wa4[j]; } +#if 0 + std::cout << "qtf: " << qtf << std::endl; + FVectorType monqtf = qrfac.matrixQ().transpose() * fvec; + std::cout << "mon qtf :" << monqtf << std::endl; +#endif + /* compute the norm of the scaled gradient. */ gnorm = 0.; @@ -540,10 +551,16 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep( wa2[j] = fjac.col(j).head(j).stableNorm(); } if (sing) { - ipvt.array() += 1; wa2 = fjac.colwise().blueNorm(); - ei_qrfac<Scalar>(n, n, fjac.data(), fjac.rows(), true, ipvt.data(), wa1.data()); - ipvt.array() -= 1; // qrfac() creates ipvt with fortran convention (1->n), convert it to c (0->n-1) + // TODO We have no unit test covering this branch.. untested + ColPivHouseholderQR<JacobianType> qrfac(fjac); + fjac = qrfac.matrixQR(); + wa1 = fjac.diagonal(); + fjac.diagonal() = qrfac.hCoeffs(); + ipvt = qrfac.colsPermutation().indices(); + // TODO : avoid this: + for(int ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors + for (j = 0; j < n; ++j) { if (fjac(j,j) != 0.) { sum = 0.; diff --git a/unsupported/Eigen/src/NonLinearOptimization/qrfac.h b/unsupported/Eigen/src/NonLinearOptimization/qrfac.h deleted file mode 100644 index ff297293d..000000000 --- a/unsupported/Eigen/src/NonLinearOptimization/qrfac.h +++ /dev/null @@ -1,105 +0,0 @@ - -template <typename Scalar> -void ei_qrfac(int m, int n, Scalar *a, int - lda, int pivot, int *ipvt, Scalar *rdiag) -{ - /* System generated locals */ - int a_dim1, a_offset; - - /* Local variables */ - int i, j, k, jp1; - Scalar sum; - int kmax; - Scalar temp; - int minmn; - Scalar ajnorm; - - Matrix< Scalar, Dynamic, 1 > wa(n+1); - - /* Parameter adjustments */ - --rdiag; - a_dim1 = lda; - a_offset = 1 + a_dim1 * 1; - a -= a_offset; - --ipvt; - - /* Function Body */ - const Scalar epsmch = epsilon<Scalar>(); - - /* compute the initial column norms and initialize several arrays. */ - - for (j = 1; j <= n; ++j) { - rdiag[j] = Map< Matrix< Scalar, Dynamic, 1 > >(&a[j * a_dim1 + 1],m).blueNorm(); - wa[j] = rdiag[j]; - if (pivot) - ipvt[j] = j; - } - - /* reduce a to r with householder transformations. */ - - minmn = std::min(m,n); - for (j = 1; j <= minmn; ++j) { - if (! (pivot)) - goto L40; - - /* bring the column of largest norm into the pivot position. */ - - kmax = j; - for (k = j; k <= n; ++k) - if (rdiag[k] > rdiag[kmax]) - kmax = k; - if (kmax == j) - goto L40; - for (i = 1; i <= m; ++i) { - temp = a[i + j * a_dim1]; - a[i + j * a_dim1] = a[i + kmax * a_dim1]; - a[i + kmax * a_dim1] = temp; - /* L30: */ - } - rdiag[kmax] = rdiag[j]; - wa[kmax] = wa[j]; - k = ipvt[j]; - ipvt[j] = ipvt[kmax]; - ipvt[kmax] = k; -L40: - - /* compute the householder transformation to reduce the */ - /* j-th column of a to a multiple of the j-th unit vector. */ - - ajnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&a[j + j * a_dim1],m-j+1).blueNorm(); - if (ajnorm == 0.) - goto L100; - if (a[j + j * a_dim1] < 0.) - ajnorm = -ajnorm; - for (i = j; i <= m; ++i) - a[i + j * a_dim1] /= ajnorm; - a[j + j * a_dim1] += 1.; - - /* apply the transformation to the remaining columns */ - /* and update the norms. */ - - jp1 = j + 1; - for (k = jp1; k <= n; ++k) { - sum = 0.; - for (i = j; i <= m; ++i) - sum += a[i + j * a_dim1] * a[i + k * a_dim1]; - temp = sum / a[j + j * a_dim1]; - for (i = j; i <= m; ++i) - a[i + k * a_dim1] -= temp * a[i + j * a_dim1]; - if (! (pivot) || rdiag[k] == 0.) - continue; - temp = a[j + k * a_dim1] / rdiag[k]; - /* Computing MAX */ - /* Computing 2nd power */ - rdiag[k] *= ei_sqrt((std::max(Scalar(0.), Scalar(1.)-ei_abs2(temp)))); - /* Computing 2nd power */ - if (Scalar(.05) * ei_abs2(rdiag[k] / wa[k]) > epsmch) - continue; - rdiag[k] = Map< Matrix< Scalar, Dynamic, 1 > >(&a[jp1 + k * a_dim1],m-j).blueNorm(); - wa[k] = rdiag[k]; - } -L100: - rdiag[j] = -ajnorm; - } -} - diff --git a/unsupported/test/NonLinearOptimization.cpp b/unsupported/test/NonLinearOptimization.cpp index ae587f016..baca18052 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.429604433690E-25); // should be 1.4307867721E-25, but nist results are on 128-bit floats + VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.428127827535E-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 ); @@ -1031,7 +1031,7 @@ void testNistLanczos1(void) VERIFY( 9 == lm.nfev); VERIFY( 8 == lm.njev); // check norm^2 - VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.43049947737308E-25); // should be 1.4307867721E-25, but nist results are on 128-bit floats + VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.43059335827267E-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 ); @@ -1170,9 +1170,9 @@ void testNistMGH10(void) info = lm.minimize(x); // check return value - VERIFY( 2 == info); - VERIFY( 285 == lm.nfev); - VERIFY( 250 == lm.njev); + VERIFY( 3 == info); + VERIFY( 281 == lm.nfev); + VERIFY( 248 == lm.njev); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.7945855171E+01); // check x @@ -1269,9 +1269,9 @@ void testNistBoxBOD(void) info = lm.minimize(x); // check return value - VERIFY( 1 == info); - VERIFY( 15 == lm.nfev); - VERIFY( 14 == lm.njev); + VERIFY( 1 == info); + VERIFY( 17 == lm.nfev); + VERIFY( 14 == lm.njev); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.1680088766E+03); // check x @@ -1331,9 +1331,9 @@ void testNistMGH17(void) info = lm.minimize(x); // check return value - VERIFY( 1 == info); - VERIFY( 599 == lm.nfev); - VERIFY( 544 == lm.njev); + VERIFY( 2 == info); + VERIFY( 603 == lm.nfev); + VERIFY( 544 == lm.njev); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.4648946975E-05); // check x @@ -1418,16 +1418,16 @@ void testNistMGH09(void) info = lm.minimize(x); // check return value - VERIFY( 1 == info); - VERIFY( 503== lm.nfev); - VERIFY( 385 == lm.njev); + VERIFY( 1 == info); + VERIFY( 494== lm.nfev); + VERIFY( 382 == lm.njev); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 3.0750560385E-04); // check x - VERIFY_IS_APPROX(x[0], 0.19280624); // should be 1.9280693458E-01 - VERIFY_IS_APPROX(x[1], 0.19129774); // should be 1.9128232873E-01 - VERIFY_IS_APPROX(x[2], 0.12305940); // should be 1.2305650693E-01 - VERIFY_IS_APPROX(x[3], 0.13606946); // should be 1.3606233068E-01 + VERIFY_IS_APPROX(x[0], 0.192807830); // should be 1.9280693458E-01 + VERIFY_IS_APPROX(x[1], 0.191262206); // should be 1.9128232873E-01 + VERIFY_IS_APPROX(x[2], 0.123052716); // should be 1.2305650693E-01 + VERIFY_IS_APPROX(x[3], 0.136053014); // should be 1.3606233068E-01 /* * Second try @@ -1833,7 +1833,6 @@ void test_NonLinearOptimization() /* * Can be useful for debugging... - printf("info, nfev, njev : %d, %d, %d\n", info, lm.nfev, lm.njev); printf("info, nfev : %d, %d\n", info, lm.nfev); printf("info, nfev, njev : %d, %d, %d\n", info, solver.nfev, solver.njev); printf("info, nfev : %d, %d\n", info, solver.nfev); @@ -1843,5 +1842,10 @@ void test_NonLinearOptimization() printf("x[3] : %.32g\n", x[3]); printf("fvec.blueNorm() : %.32g\n", solver.fvec.blueNorm()); printf("fvec.blueNorm() : %.32g\n", lm.fvec.blueNorm()); + + printf("info, nfev, njev : %d, %d, %d\n", info, lm.nfev, lm.njev); + printf("fvec.blueNorm() : %.32g\n", lm.fvec.blueNorm()); + printf("fvec.squaredNorm() : %.32g\n", lm.fvec.squaredNorm()); + std::cout << x << std::endl; */ |