Replace the qr factorization from (c)minpack (qrfac) by Eigen's own stuff.

Results as checked by unit tests are very slightly worse, but not much.

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;
 */