1 files changed, 189 insertions, 0 deletions
diff --git a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h b/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h
new file mode 100644
index 0000000..f5290de
--- /dev/null
+++ b/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h
@@ -0,0 +1,189 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
+// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
+//
+// This code initially comes from MINPACK whose original authors are:
+// Copyright Jorge More - Argonne National Laboratory
+// Copyright Burt Garbow - Argonne National Laboratory
+// Copyright Ken Hillstrom - Argonne National Laboratory
+//
+// This Source Code Form is subject to the terms of the Minpack license
+// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
+
+#ifndef EIGEN_LMQRSOLV_H
+#define EIGEN_LMQRSOLV_H
+
+namespace Eigen { 
+
+namespace internal {
+
+template <typename Scalar,int Rows, int Cols, typename Index>
+void lmqrsolv(
+  Matrix<Scalar,Rows,Cols> &s,
+  const PermutationMatrix<Dynamic,Dynamic,Index> &iPerm,
+  const Matrix<Scalar,Dynamic,1> &diag,
+  const Matrix<Scalar,Dynamic,1> &qtb,
+  Matrix<Scalar,Dynamic,1> &x,
+  Matrix<Scalar,Dynamic,1> &sdiag)
+{
+
+    /* Local variables */
+    Index i, j, k, l;
+    Scalar temp;
+    Index n = s.cols();
+    Matrix<Scalar,Dynamic,1>  wa(n);
+    JacobiRotation<Scalar> givens;
+
+    /* Function Body */
+    // the following will only change the lower triangular part of s, including
+    // the diagonal, though the diagonal is restored afterward
+
+    /*     copy r and (q transpose)*b to preserve input and initialize s. */
+    /*     in particular, save the diagonal elements of r in x. */
+    x = s.diagonal();
+    wa = qtb;
+    
+   
+    s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose();
+    /*     eliminate the diagonal matrix d using a givens rotation. */
+    for (j = 0; j < n; ++j) {
+
+        /*        prepare the row of d to be eliminated, locating the */
+        /*        diagonal element using p from the qr factorization. */
+        l = iPerm.indices()(j);
+        if (diag[l] == 0.)
+            break;
+        sdiag.tail(n-j).setZero();
+        sdiag[j] = diag[l];
+
+        /*        the transformations to eliminate the row of d */
+        /*        modify only a single element of (q transpose)*b */
+        /*        beyond the first n, which is initially zero. */
+        Scalar qtbpj = 0.;
+        for (k = j; k < n; ++k) {
+            /*           determine a givens rotation which eliminates the */
+            /*           appropriate element in the current row of d. */
+            givens.makeGivens(-s(k,k), sdiag[k]);
+
+            /*           compute the modified diagonal element of r and */
+            /*           the modified element of ((q transpose)*b,0). */
+            s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k];
+            temp = givens.c() * wa[k] + givens.s() * qtbpj;
+            qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
+            wa[k] = temp;
+
+            /*           accumulate the tranformation in the row of s. */
+            for (i = k+1; i<n; ++i) {
+                temp = givens.c() * s(i,k) + givens.s() * sdiag[i];
+                sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i];
+                s(i,k) = temp;
+            }
+        }
+    }
+  
+    /*     solve the triangular system for z. if the system is */
+    /*     singular, then obtain a least squares solution. */
+    Index nsing;
+    for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {}
+
+    wa.tail(n-nsing).setZero();
+    s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
+  
+    // restore
+    sdiag = s.diagonal();
+    s.diagonal() = x;
+
+    /* permute the components of z back to components of x. */
+    x = iPerm * wa; 
+}
+
+template <typename Scalar, int _Options, typename Index>
+void lmqrsolv(
+  SparseMatrix<Scalar,_Options,Index> &s,
+  const PermutationMatrix<Dynamic,Dynamic> &iPerm,
+  const Matrix<Scalar,Dynamic,1> &diag,
+  const Matrix<Scalar,Dynamic,1> &qtb,
+  Matrix<Scalar,Dynamic,1> &x,
+  Matrix<Scalar,Dynamic,1> &sdiag)
+{
+  /* Local variables */
+  typedef SparseMatrix<Scalar,RowMajor,Index> FactorType;
+    Index i, j, k, l;
+    Scalar temp;
+    Index n = s.cols();
+    Matrix<Scalar,Dynamic,1>  wa(n);
+    JacobiRotation<Scalar> givens;
+
+    /* Function Body */
+    // the following will only change the lower triangular part of s, including
+    // the diagonal, though the diagonal is restored afterward
+
+    /*     copy r and (q transpose)*b to preserve input and initialize R. */
+    wa = qtb;
+    FactorType R(s);
+    // Eliminate the diagonal matrix d using a givens rotation
+    for (j = 0; j < n; ++j)
+    {
+      // Prepare the row of d to be eliminated, locating the 
+      // diagonal element using p from the qr factorization
+      l = iPerm.indices()(j);
+      if (diag(l) == Scalar(0)) 
+        break; 
+      sdiag.tail(n-j).setZero();
+      sdiag[j] = diag[l];
+      // the transformations to eliminate the row of d
+      // modify only a single element of (q transpose)*b
+      // beyond the first n, which is initially zero. 
+      
+      Scalar qtbpj = 0; 
+      // Browse the nonzero elements of row j of the upper triangular s
+      for (k = j; k < n; ++k)
+      {
+        typename FactorType::InnerIterator itk(R,k);
+        for (; itk; ++itk){
+          if (itk.index() < k) continue;
+          else break;
+        }
+        //At this point, we have the diagonal element R(k,k)
+        // Determine a givens rotation which eliminates 
+        // the appropriate element in the current row of d
+        givens.makeGivens(-itk.value(), sdiag(k));
+        
+        // Compute the modified diagonal element of r and 
+        // the modified element of ((q transpose)*b,0).
+        itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k);
+        temp = givens.c() * wa(k) + givens.s() * qtbpj; 
+        qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj;
+        wa(k) = temp;
+        
+        // Accumulate the transformation in the remaining k row/column of R
+        for (++itk; itk; ++itk)
+        {
+          i = itk.index();
+          temp = givens.c() *  itk.value() + givens.s() * sdiag(i);
+          sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i);
+          itk.valueRef() = temp;
+        }
+      }
+    }
+    
+    // Solve the triangular system for z. If the system is 
+    // singular, then obtain a least squares solution
+    Index nsing;
+    for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {}
+    
+    wa.tail(n-nsing).setZero();
+//     x = wa; 
+    wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing));
+    
+    sdiag = R.diagonal();
+    // Permute the components of z back to components of x
+    x = iPerm * wa; 
+}
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_LMQRSOLV_H