misc cleaning / eigenization

8 files changed, 44 insertions, 61 deletions

diff --git a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
index 539c58d72..51cea50be 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
@@ -238,8 +238,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
 
         /* on the first iteration, calculate the norm of the scaled x */
         /* and initialize the step bound delta. */
-        wa3 = diag.cwiseProduct(x);
-        xnorm = wa3.stableNorm();
+        xnorm = diag.cwiseProduct(x).stableNorm();
         delta = parameters.factor * xnorm;
         if (delta == 0.)
             delta = parameters.factor;
@@ -269,8 +268,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
         /* store the direction p and x + p. calculate the norm of p. */
         wa1 = -wa1;
         wa2 = x + wa1;
-        wa3 = diag.cwiseProduct(wa1);
-        pnorm = wa3.stableNorm();
+        pnorm = diag.cwiseProduct(wa1).stableNorm();
 
         /* on the first iteration, adjust the initial step bound. */
         if (iter == 1)
@@ -489,8 +487,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
 
         /* on the first iteration, calculate the norm of the scaled x */
         /* and initialize the step bound delta. */
-        wa3 = diag.cwiseProduct(x);
-        xnorm = wa3.stableNorm();
+        xnorm = diag.cwiseProduct(x).stableNorm();
         delta = parameters.factor * xnorm;
         if (delta == 0.)
             delta = parameters.factor;
@@ -520,8 +517,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
         /* store the direction p and x + p. calculate the norm of p. */
         wa1 = -wa1;
         wa2 = x + wa1;
-        wa3 = diag.cwiseProduct(wa1);
-        pnorm = wa3.stableNorm();
+        pnorm = diag.cwiseProduct(wa1).stableNorm();
 
         /* on the first iteration, adjust the initial step bound. */
         if (iter == 1)
diff --git a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
index cda6ef74b..076110651 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
@@ -261,8 +261,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(
 
         /* on the first iteration, calculate the norm of the scaled x */
         /* and initialize the step bound delta. */
-        wa3 = diag.cwiseProduct(x);
-        xnorm = wa3.stableNorm();
+        xnorm = diag.cwiseProduct(x).stableNorm();
         delta = parameters.factor * xnorm;
         if (delta == 0.)
             delta = parameters.factor;
@@ -297,8 +296,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(
         /* store the direction p and x + p. calculate the norm of p. */
         wa1 = -wa1;
         wa2 = x + wa1;
-        wa3 = diag.cwiseProduct(wa1);
-        pnorm = wa3.stableNorm();
+        pnorm = diag.cwiseProduct(wa1).stableNorm();
 
         /* on the first iteration, adjust the initial step bound. */
         if (iter == 1)
@@ -515,8 +513,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
 
         /* on the first iteration, calculate the norm of the scaled x */
         /* and initialize the step bound delta. */
-        wa3 = diag.cwiseProduct(x);
-        xnorm = wa3.stableNorm();
+        xnorm = diag.cwiseProduct(x).stableNorm();
         delta = parameters.factor * xnorm;
         if (delta == 0.)
             delta = parameters.factor;
@@ -545,8 +542,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
         /* store the direction p and x + p. calculate the norm of p. */
         wa1 = -wa1;
         wa2 = x + wa1;
-        wa3 = diag.cwiseProduct(wa1);
-        pnorm = wa3.stableNorm();
+        pnorm = diag.cwiseProduct(wa1).stableNorm();
 
         /* on the first iteration, adjust the initial step bound. */
         if (iter == 1)
diff --git a/unsupported/Eigen/src/NonLinearOptimization/chkder.h b/unsupported/Eigen/src/NonLinearOptimization/chkder.h
index 21fd60a9d..be8115230 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/chkder.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/chkder.h
@@ -4,11 +4,11 @@
 
 template<typename Scalar>
 void ei_chkder(
-        Matrix< Scalar, Dynamic, 1 >  &x,
-        Matrix< Scalar, Dynamic, 1 >  &fvec,
-        Matrix< Scalar, Dynamic, Dynamic > &fjac,
+        const Matrix< Scalar, Dynamic, 1 >  &x,
+        const Matrix< Scalar, Dynamic, 1 >  &fvec,
+        const Matrix< Scalar, Dynamic, Dynamic > &fjac,
         Matrix< Scalar, Dynamic, 1 >  &xp,
-        Matrix< Scalar, Dynamic, 1 >  &fvecp,
+        const Matrix< Scalar, Dynamic, 1 >  &fvecp,
         int mode,
         Matrix< Scalar, Dynamic, 1 >  &err
         )
diff --git a/unsupported/Eigen/src/NonLinearOptimization/covar.h b/unsupported/Eigen/src/NonLinearOptimization/covar.h
index 620a2d71a..15c72d6ed 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/covar.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/covar.h
@@ -16,7 +16,7 @@ void ei_covar(
     Matrix< Scalar, Dynamic, 1 > wa(n);
     assert(ipvt.size()==n);
 
-    /*     form the inverse of r in the full upper triangle of r. */
+    /* form the inverse of r in the full upper triangle of r. */
     l = -1;
     for (k = 0; k < n; ++k)
         if (ei_abs(r(k,k)) > tolr) {
@@ -24,27 +24,21 @@ void ei_covar(
             for (j = 0; j <= k-1; ++j) {
                 temp = r(k,k) * r(j,k);
                 r(j,k) = 0.;
-                for (i = 0; i <= j; ++i)
-                    r(i,k) -= temp * r(i,j);
+                r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
             }
             l = k;
         }
 
-    /*     form the full upper triangle of the inverse of (r transpose)*r */
-    /*     in the full upper triangle of r. */
+    /* form the full upper triangle of the inverse of (r transpose)*r */
+    /* in the full upper triangle of r. */
     for (k = 0; k <= l; ++k) {
-        for (j = 0; j <= k-1; ++j) {
-            temp = r(j,k);
-            for (i = 0; i <= j; ++i)
-                r(i,j) += temp * r(i,k);
-        }
-        temp = r(k,k);
-        for (i = 0; i <= k; ++i)
-            r(i,k) = temp * r(i,k);
+        for (j = 0; j <= k-1; ++j)
+            r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
+        r.col(k).head(k+1) *= r(k,k);
     }
 
-    /*     form the full lower triangle of the covariance matrix */
-    /*     in the strict lower triangle of r and in wa. */
+    /* form the full lower triangle of the covariance matrix */
+    /* in the strict lower triangle of r and in wa. */
     for (j = 0; j < n; ++j) {
         jj = ipvt[j];
         sing = j > l;
@@ -60,11 +54,8 @@ void ei_covar(
         wa[jj] = r(j,j);
     }
 
-    /*     symmetrize the covariance matrix in r. */
-    for (j = 0; j < n; ++j) {
-        for (i = 0; i <= j; ++i)
-            r(i,j) = r(j,i);
-        r(j,j) = wa[j];
-    }
+    /* symmetrize the covariance matrix in r. */
+    r.corner(TopLeft,n,n).template triangularView<StrictlyUpper>() = r.corner(TopLeft,n,n).transpose();
+    r.diagonal() = wa;
 }
 
diff --git a/unsupported/Eigen/src/NonLinearOptimization/dogleg.h b/unsupported/Eigen/src/NonLinearOptimization/dogleg.h
index 18b0963b2..b3c4fbb96 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/dogleg.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/dogleg.h
@@ -36,8 +36,7 @@ void ei_dogleg(
     }
 
     /* test whether the gauss-newton direction is acceptable. */
-    wa2 = diag.cwiseProduct(x);
-    qnorm = wa2.stableNorm();
+    qnorm = diag.cwiseProduct(x).stableNorm();
     if (qnorm <= delta)
         return;
 
@@ -48,9 +47,7 @@ void ei_dogleg(
 
     wa1.fill(0.);
     for (j = 0; j < n; ++j) {
-        temp = qtb[j];
-        for (i = j; i < n; ++i)
-            wa1[i] += qrfac(j,i) * temp;
+        wa1.tail(n-j) += qrfac.row(j).tail(n-j) * qtb[j];
         wa1[j] /= diag[j];
     }
 
diff --git a/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h b/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h
index 77c18fbeb..9564a784b 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h
@@ -10,10 +10,11 @@ int ei_fdjac1(
 {
     /* Local variables */
     Scalar h;
-    int i, j, k;
+    int j, k;
     Scalar eps, temp;
     int msum;
     int iflag;
+    int start, length;
 
     /* Function Body */
     const Scalar epsmch = epsilon<Scalar>();
@@ -55,11 +56,10 @@ int ei_fdjac1(
                 x[j] = wa2[j];
                 h = eps * ei_abs(wa2[j]);
                 if (h == 0.) h = eps;
-                for (i = 0; i < n; ++i) {
-                    fjac(i,j) = 0.;
-                    if (i >= j - mu && i <= j + ml)
-                        fjac(i,j) = (wa1[i] - fvec[i]) / h;
-                }
+                fjac.col(j).setZero();
+                start = std::max(0,j-mu);
+                length = std::min(n-1, j+ml) - start + 1;
+                fjac.col(j).segment(start, length) = ( wa1.segment(start, length)-fvec.segment(start, length))/h;
             }
         }
     }
diff --git a/unsupported/Eigen/src/NonLinearOptimization/lmpar.h b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h
index cd4698d76..d763bd4e7 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/lmpar.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h
@@ -164,7 +164,7 @@ void ei_lmpar2(
 
 {
     /* Local variables */
-    int i, j;
+    int j;
     Scalar fp;
     Scalar parc, parl;
     int iter;
@@ -183,9 +183,11 @@ void ei_lmpar2(
 
     /* compute and store in x the gauss-newton direction. if the */
     /* jacobian is rank-deficient, obtain a least squares solution. */
+
 //    const int rank = qr.nonzeroPivots(); // exactly double(0.)
     const int rank = qr.rank(); // use a threshold
-    wa1 = qtb; wa1.segment(rank,n-rank).setZero();
+    wa1 = qtb;
+    wa1.tail(n-rank).setZero();
     qr.matrixQR().corner(TopLeft, rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));
 
     x = qr.colsPermutation()*wa1;
@@ -255,10 +257,12 @@ void ei_lmpar2(
 
         /* compute the newton correction. */
         wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm);
+        // we could almost use this here, but the diagonal is outside qr, in sdiag[]
+        // qr.matrixQR().corner(TopLeft, n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
         for (j = 0; j < n; ++j) {
             wa1[j] /= sdiag[j];
             temp = wa1[j];
-            for (i = j+1; i < n; ++i)
+            for (int i = j+1; i < n; ++i)
                 wa1[i] -= s(i,j) * temp;
         }
         temp = wa1.blueNorm();
diff --git a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
index 18e313c27..f89a5f9a8 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
@@ -1,4 +1,5 @@
 
+// TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt
 template <typename Scalar>
 void ei_qrsolv(
         Matrix< Scalar, Dynamic, Dynamic > &s,
@@ -15,6 +16,7 @@ void ei_qrsolv(
     Scalar temp;
     int n = s.cols();
     Matrix< Scalar, Dynamic, 1 >  wa(n);
+    PlanarRotation<Scalar> givens;
 
     /* Function Body */
     // the following will only change the lower triangular part of s, including
@@ -25,9 +27,7 @@ void ei_qrsolv(
     x = s.diagonal();
     wa = qtb;
 
-    for (j = 0; j < n; ++j)
-        for (i = j+1; i < n; ++i)
-            s(i,j) = s(j,i);
+    s.corner(TopLeft,n,n).template triangularView<StrictlyLower>() = s.corner(TopLeft,n,n).transpose();
 
     /*     eliminate the diagonal matrix d using a givens rotation. */
     for (j = 0; j < n; ++j) {
@@ -37,7 +37,7 @@ void ei_qrsolv(
         l = ipvt[j];
         if (diag[l] == 0.)
             break;
-        sdiag.segment(j,n-j).setZero();
+        sdiag.tail(n-j).setZero();
         sdiag[j] = diag[l];
 
         /*        the transformations to eliminate the row of d */
@@ -47,7 +47,6 @@ void ei_qrsolv(
         for (k = j; k < n; ++k) {
             /*           determine a givens rotation which eliminates the */
             /*           appropriate element in the current row of d. */
-            PlanarRotation<Scalar> givens;
             givens.makeGivens(-s(k,k), sdiag[k]);
 
             /*           compute the modified diagonal element of r and */
@@ -70,8 +69,8 @@ void ei_qrsolv(
     /*     singular, then obtain a least squares solution. */
     int nsing;
     for (nsing=0; nsing<n && sdiag[nsing]!=0; nsing++);
-    wa.segment(nsing,n-nsing).setZero();
 
+    wa.tail(n-nsing).setZero();
     s.corner(TopLeft, nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
 
     // restore