cleaning

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) {
+    for (j = nsing; j>=0; --j) {
-        j = nsing - k;
         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) {
+    for (j = 0; j < n; ++j)
-        l = ipvt[j];
+        x[ipvt[j]] = wa1[j];
-        x[l] = 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) {
+    for (j = 0; j < n; ++j)
-        sum = 0.;
+        wa1[j] = r.col(j).start(j+1).dot(qtb.start(j+1)) / diag[ipvt[j]];
-        for (i = 0; i <= j; ++i)
+
-            sum += r(i,j) * qtb[i];
-        l = ipvt[j];
-        wa1[j] = sum / diag[l];
-    }
     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 = ei_sqrt(par)* diag;
-        wa1 = temp * 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];

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