From 7ba9dc07edb356d1a7d81bf4ef62378099a509ca Mon Sep 17 00:00:00 2001
From: Thomas Capricelli <orzel@freehackers.org>
Date: Wed, 27 Jan 2010 09:01:13 +0100
Subject: [PATCH] port ei_rwupdt to c++,  and misc cleaning

---
 .../LevenbergMarquardt.h                      | 14 +++--
 .../Eigen/src/NonLinearOptimization/rwupdt.h  | 52 +++++++++----------
 2 files changed, 30 insertions(+), 36 deletions(-)

diff --git a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
index adbb3c835..cda6ef74b 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
@@ -289,7 +289,6 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(
     if (mode != 2)
         diag = diag.cwiseMax(wa2);
 
-    /* beginning of the inner loop. */
     do {
 
         /* determine the levenberg-marquardt parameter. */
@@ -374,9 +373,9 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(
             return XtolTooSmall;
         if (gnorm <= epsilon<Scalar>())
             return GtolTooSmall;
-        /* end of the inner loop. repeat if iteration unsuccessful. */
+
     } while (ratio < Scalar(1e-4));
-    /* end of the outer loop. */
+
     return Running;
 }
 
@@ -468,7 +467,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
     int rownb = 2;
     for (i = 0; i < m; ++i) {
         if (functor.df(x, wa3, rownb) < 0) return UserAsked;
-        ei_rwupdt<Scalar>(n, fjac.data(), fjac.rows(), wa3.data(), qtf.data(), fvec[i]);
+        ei_rwupdt<Scalar>(fjac, wa3, qtf, fvec[i]);
         ++rownb;
     }
     ++njev;
@@ -485,7 +484,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
     if (sing) {
         wa2 = fjac.colwise().blueNorm();
         // TODO We have no unit test covering this code path, do not modify
-        // before it is carefully tested
+        // until it is carefully tested
         ColPivHouseholderQR<JacobianType> qrfac(fjac);
         fjac = qrfac.matrixQR();
         wa1 = fjac.diagonal();
@@ -538,7 +537,6 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
     if (mode != 2)
         diag = diag.cwiseMax(wa2);
 
-    /* beginning of the inner loop. */
     do {
 
         /* determine the levenberg-marquardt parameter. */
@@ -623,9 +621,9 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
             return XtolTooSmall;
         if (gnorm <= epsilon<Scalar>())
             return GtolTooSmall;
-        /* end of the inner loop. repeat if iteration unsuccessful. */
+
     } while (ratio < Scalar(1e-4));
-    /* end of the outer loop. */
+
     return Running;
 }
 
diff --git a/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h b/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h
index 38676e720..7703ff8de 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h
@@ -1,45 +1,41 @@
 
-    template <typename Scalar>
-void ei_rwupdt(int n, Scalar *r__, int ldr, const Scalar *w, Scalar *b, Scalar alpha)
+template <typename Scalar>
+void ei_rwupdt(
+        Matrix< Scalar, Dynamic, Dynamic >  &r,
+        const Matrix< Scalar, Dynamic, 1>  &w,
+        Matrix< Scalar, Dynamic, 1>  &b,
+        Scalar alpha)
 {
+    const int n = r.cols();
+    assert(r.rows()>=n);
     std::vector<PlanarRotation<Scalar> > givens(n);
-    /* System generated locals */
-    int r_dim1, r_offset;
 
     /* Local variables */
     Scalar temp, rowj;
 
-    /* Parameter adjustments */
-    --b;
-    --w;
-    r_dim1 = ldr;
-    r_offset = 1 + r_dim1 * 1;
-    r__ -= r_offset;
-
     /* Function Body */
-    for (int j = 1; j <= n; ++j) {
+    for (int j = 0; j < n; ++j) {
         rowj = w[j];
 
         /* apply the previous transformations to */
-        /* r(i,j), i=1,2,...,j-1, and to w(j). */
-        if (j-1>=1)
-            for (int i = 1; i <= j-1; ++i) {
-                temp = givens[i-1].c() * r__[i + j * r_dim1] + givens[i-1].s() * rowj;
-                rowj = -givens[i-1].s() * r__[i + j * r_dim1] + givens[i-1].c() * rowj;
-                r__[i + j * r_dim1] = temp;
-            }
+        /* r(i,j), i=0,1,...,j-1, and to w(j). */
+        for (int i = 0; i < j; ++i) {
+            temp = givens[i].c() * r(i,j) + givens[i].s() * rowj;
+            rowj = -givens[i].s() * r(i,j) + givens[i].c() * rowj;
+            r(i,j) = temp;
+        }
+
+        if (rowj == 0.)
+            continue;
 
         /* determine a givens rotation which eliminates w(j). */
-        if (rowj != 0.) {
-            givens[j-1].makeGivens(-r__[j + j * r_dim1], rowj);
+        givens[j].makeGivens(-r(j,j), rowj);
 
-            /* apply the current transformation to r(j,j), b(j), and alpha. */
-            r__[j + j * r_dim1] = givens[j-1].c() * r__[j + j * r_dim1] + givens[j-1].s() * rowj;
-            temp = givens[j-1].c() * b[j] + givens[j-1].s() * alpha;
-            alpha = -givens[j-1].s() * b[j] + givens[j-1].c() * alpha;
-            b[j] = temp;
-        }
+        /* apply the current transformation to r(j,j), b(j), and alpha. */
+        r(j,j) = givens[j].c() * r(j,j) + givens[j].s() * rowj;
+        temp = givens[j].c() * b[j] + givens[j].s() * alpha;
+        alpha = -givens[j].s() * b[j] + givens[j].c() * alpha;
+        b[j] = temp;
     }
-    return;
 }