Add missing minpack copyrights/license.

Fix LM header files and credits original MINPACK authors.
Move minimizeOneStep code into its own file to get it more properly credited.

7 files changed, 430 insertions, 307 deletions

diff --git a/Eigen/LevenbergMarquardt b/Eigen/LevenbergMarquardt
index 1a7440ad9..5ce1a23f6 100644
--- a/Eigen/LevenbergMarquardt
+++ b/Eigen/LevenbergMarquardt
@@ -34,12 +34,13 @@
 #ifndef EIGEN_PARSED_BY_DOXYGEN
 
 #include "src/LevenbergMarquardt/LMqrsolv.h"
-#include "src/LevenbergMarquardt/covar.h"
+#include "src/LevenbergMarquardt/LMcovar.h"
+#include "src/LevenbergMarquardt/LMpar.h"
 
 #endif
 
 #include "src/LevenbergMarquardt/LevenbergMarquardt.h"
-
+#include "src/LevenbergMarquardt/LMonestep.h"
 
 
 #endif // EIGEN_LEVENBERGMARQUARDT_MODULE
diff --git a/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt b/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt
new file mode 100644
index 000000000..ae7984dae
--- /dev/null
+++ b/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt
@@ -0,0 +1,52 @@
+Minpack Copyright Notice (1999) University of Chicago.  All rights reserved
+
+Redistribution and use in source and binary forms, with or
+without modification, are permitted provided that the
+following conditions are met:
+
+1. Redistributions of source code must retain the above
+copyright notice, this list of conditions and the following
+disclaimer.
+
+2. Redistributions in binary form must reproduce the above
+copyright notice, this list of conditions and the following
+disclaimer in the documentation and/or other materials
+provided with the distribution.
+
+3. The end-user documentation included with the
+redistribution, if any, must include the following
+acknowledgment:
+
+   "This product includes software developed by the
+   University of Chicago, as Operator of Argonne National
+   Laboratory.
+
+Alternately, this acknowledgment may appear in the software
+itself, if and wherever such third-party acknowledgments
+normally appear.
+
+4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS"
+WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE
+UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND
+THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES
+OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE
+OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY
+OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR
+USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF
+THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4)
+DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION
+UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL
+BE CORRECTED.
+
+5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT
+HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF
+ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT,
+INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF
+ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF
+PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER
+SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT
+(INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE,
+EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE
+POSSIBILITY OF SUCH LOSS OR DAMAGES.
+
diff --git a/Eigen/src/LevenbergMarquardt/covar.h b/Eigen/src/LevenbergMarquardt/LMcovar.h
index 1771ad1ff..3210587e4 100644
--- a/Eigen/src/LevenbergMarquardt/covar.h
+++ b/Eigen/src/LevenbergMarquardt/LMcovar.h
@@ -1,3 +1,17 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// 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_LMCOVAR_H
+#define EIGEN_LMCOVAR_H
+
 namespace Eigen { 
 
 namespace internal {
@@ -67,3 +81,5 @@ void covar(
 } // end namespace internal
 
 } // end namespace Eigen
+
+#endif // EIGEN_LMCOVAR_H
diff --git a/Eigen/src/LevenbergMarquardt/LMonestep.h b/Eigen/src/LevenbergMarquardt/LMonestep.h
new file mode 100644
index 000000000..aa0c02668
--- /dev/null
+++ b/Eigen/src/LevenbergMarquardt/LMonestep.h
@@ -0,0 +1,179 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
+//
+// 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_LMONESTEP_H
+#define EIGEN_LMONESTEP_H
+
+namespace Eigen {
+
+template<typename FunctorType>
+LevenbergMarquardtSpace::Status
+LevenbergMarquardt<FunctorType>::minimizeOneStep(FVectorType  &x)
+{
+  typedef typename FunctorType::JacobianType JacobianType; 
+  using std::abs;
+  using std::sqrt;
+  RealScalar temp, temp1,temp2; 
+  RealScalar ratio; 
+  RealScalar pnorm, xnorm, fnorm1, actred, dirder, prered;
+  assert(x.size()==n); // check the caller is not cheating us
+
+  temp = 0.0; xnorm = 0.0;
+  /* calculate the jacobian matrix. */
+  Index df_ret = m_functor.df(x, m_fjac);
+  if (df_ret<0)
+      return LevenbergMarquardtSpace::UserAsked;
+  if (df_ret>0)
+      // numerical diff, we evaluated the function df_ret times
+      m_nfev += df_ret;
+  else m_njev++;
+
+  /* compute the qr factorization of the jacobian. */
+  for (int j = 0; j < x.size(); ++j)
+    m_wa2(j) = m_fjac.col(j).norm(); 
+  //FIXME Implement bluenorm for sparse vectors
+//     m_wa2 = m_fjac.colwise().blueNorm();
+  QRSolver qrfac(m_fjac); //FIXME Check if the QR decomposition succeed
+  // Make a copy of the first factor with the associated permutation
+  JacobianType rfactor;
+  rfactor = qrfac.matrixQR();
+  m_permutation = (qrfac.colsPermutation());
+
+  /* on the first iteration and if external scaling is not used, scale according */
+  /* to the norms of the columns of the initial jacobian. */
+  if (m_iter == 1) {
+      if (!m_useExternalScaling)
+          for (Index j = 0; j < n; ++j)
+              m_diag[j] = (m_wa2[j]==0.)? 1. : m_wa2[j];
+
+      /* on the first iteration, calculate the norm of the scaled x */
+      /* and initialize the step bound m_delta. */
+      xnorm = m_diag.cwiseProduct(x).stableNorm();
+      m_delta = m_factor * xnorm;
+      if (m_delta == 0.)
+          m_delta = m_factor;
+  }
+
+  /* form (q transpose)*m_fvec and store the first n components in */
+  /* m_qtf. */
+  m_wa4 = m_fvec;
+  m_wa4 = qrfac.matrixQ().adjoint() * m_fvec; 
+  m_qtf = m_wa4.head(n);
+
+  /* compute the norm of the scaled gradient. */
+  m_gnorm = 0.;
+  if (m_fnorm != 0.)
+      for (Index j = 0; j < n; ++j)
+          if (m_wa2[m_permutation.indices()[j]] != 0.)
+              m_gnorm = (std::max)(m_gnorm, abs( rfactor.col(j).head(j+1).dot(m_qtf.head(j+1)/m_fnorm) / m_wa2[m_permutation.indices()[j]]));
+
+  /* test for convergence of the gradient norm. */
+  if (m_gnorm <= m_gtol)
+      return LevenbergMarquardtSpace::CosinusTooSmall;
+
+  /* rescale if necessary. */
+  if (!m_useExternalScaling)
+      m_diag = m_diag.cwiseMax(m_wa2);
+
+  do {
+    /* determine the levenberg-marquardt parameter. */
+    internal::lmpar2(qrfac, m_diag, m_qtf, m_delta, m_par, m_wa1);
+
+    /* store the direction p and x + p. calculate the norm of p. */
+    m_wa1 = -m_wa1;
+    m_wa2 = x + m_wa1;
+    pnorm = m_diag.cwiseProduct(m_wa1).stableNorm();
+
+    /* on the first iteration, adjust the initial step bound. */
+    if (m_iter == 1)
+        m_delta = (std::min)(m_delta,pnorm);
+
+    /* evaluate the function at x + p and calculate its norm. */
+    if ( m_functor(m_wa2, m_wa4) < 0)
+        return LevenbergMarquardtSpace::UserAsked;
+    ++m_nfev;
+    fnorm1 = m_wa4.stableNorm();
+
+    /* compute the scaled actual reduction. */
+    actred = -1.;
+    if (Scalar(.1) * fnorm1 < m_fnorm)
+        actred = 1. - internal::abs2(fnorm1 / m_fnorm);
+
+    /* compute the scaled predicted reduction and */
+    /* the scaled directional derivative. */
+    m_wa3 = rfactor.template triangularView<Upper>() * (m_permutation.inverse() *m_wa1);
+    temp1 = internal::abs2(m_wa3.stableNorm() / m_fnorm);
+    temp2 = internal::abs2(sqrt(m_par) * pnorm / m_fnorm);
+    prered = temp1 + temp2 / Scalar(.5);
+    dirder = -(temp1 + temp2);
+
+    /* compute the ratio of the actual to the predicted */
+    /* reduction. */
+    ratio = 0.;
+    if (prered != 0.)
+        ratio = actred / prered;
+
+    /* update the step bound. */
+    if (ratio <= Scalar(.25)) {
+        if (actred >= 0.)
+            temp = RealScalar(.5);
+        if (actred < 0.)
+            temp = RealScalar(.5) * dirder / (dirder + RealScalar(.5) * actred);
+        if (RealScalar(.1) * fnorm1 >= m_fnorm || temp < RealScalar(.1))
+            temp = Scalar(.1);
+        /* Computing MIN */
+        m_delta = temp * (std::min)(m_delta, pnorm / RealScalar(.1));
+        m_par /= temp;
+    } else if (!(m_par != 0. && ratio < RealScalar(.75))) {
+        m_delta = pnorm / RealScalar(.5);
+        m_par = RealScalar(.5) * m_par;
+    }
+
+    /* test for successful iteration. */
+    if (ratio >= RealScalar(1e-4)) {
+        /* successful iteration. update x, m_fvec, and their norms. */
+        x = m_wa2;
+        m_wa2 = m_diag.cwiseProduct(x);
+        m_fvec = m_wa4;
+        xnorm = m_wa2.stableNorm();
+        m_fnorm = fnorm1;
+        ++m_iter;
+    }
+
+    /* tests for convergence. */
+    if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1. && m_delta <= m_xtol * xnorm)
+        return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
+    if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1.)
+        return LevenbergMarquardtSpace::RelativeReductionTooSmall;
+    if (m_delta <= m_xtol * xnorm)
+        return LevenbergMarquardtSpace::RelativeErrorTooSmall;
+
+    /* tests for termination and stringent tolerances. */
+    if (m_nfev >= m_maxfev)
+        return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
+    if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
+        return LevenbergMarquardtSpace::FtolTooSmall;
+    if (m_delta <= NumTraits<Scalar>::epsilon() * xnorm)
+        return LevenbergMarquardtSpace::XtolTooSmall;
+    if (m_gnorm <= NumTraits<Scalar>::epsilon())
+        return LevenbergMarquardtSpace::GtolTooSmall;
+
+  } while (ratio < Scalar(1e-4));
+
+  return LevenbergMarquardtSpace::Running;
+}
+
+  
+} // end namespace Eigen
+
+#endif // EIGEN_LMONESTEP_H
\ No newline at end of file
diff --git a/Eigen/src/LevenbergMarquardt/LMpar.h b/Eigen/src/LevenbergMarquardt/LMpar.h
new file mode 100644
index 000000000..dc60ca05a
--- /dev/null
+++ b/Eigen/src/LevenbergMarquardt/LMpar.h
@@ -0,0 +1,160 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// 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_LMPAR_H
+#define EIGEN_LMPAR_H
+
+namespace Eigen {
+
+namespace internal {
+  
+  template <typename QRSolver, typename VectorType>
+    void lmpar2(
+    const QRSolver &qr,
+    const VectorType  &diag,
+    const VectorType  &qtb,
+    typename VectorType::Scalar m_delta,
+    typename VectorType::Scalar &par,
+    VectorType  &x)
+
+  {
+    using std::sqrt;
+    using std::abs;
+    typedef typename QRSolver::MatrixType MatrixType;
+    typedef typename QRSolver::Scalar Scalar;
+    typedef typename QRSolver::Index Index;
+
+    /* Local variables */
+    Index j;
+    Scalar fp;
+    Scalar parc, parl;
+    Index iter;
+    Scalar temp, paru;
+    Scalar gnorm;
+    Scalar dxnorm;
+
+
+    /* Function Body */
+    const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
+    const Index n = qr.matrixQR().cols();
+    assert(n==diag.size());
+    assert(n==qtb.size());
+
+    VectorType  wa1, wa2;
+
+    /* compute and store in x the gauss-newton direction. if the */
+    /* jacobian is rank-deficient, obtain a least squares solution. */
+
+    //    const Index rank = qr.nonzeroPivots(); // exactly double(0.)
+    const Index rank = qr.rank(); // use a threshold
+    wa1 = qtb;
+    wa1.tail(n-rank).setZero();
+    //FIXME There is no solve in place for sparse triangularView
+    //qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));
+    wa1.head(rank) = qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solve(qtb.head(rank));
+
+    x = qr.colsPermutation()*wa1;
+
+    /* initialize the iteration counter. */
+    /* evaluate the function at the origin, and test */
+    /* for acceptance of the gauss-newton direction. */
+    iter = 0;
+    wa2 = diag.cwiseProduct(x);
+    dxnorm = wa2.blueNorm();
+    fp = dxnorm - m_delta;
+    if (fp <= Scalar(0.1) * m_delta) {
+      par = 0;
+      return;
+    }
+
+    /* if the jacobian is not rank deficient, the newton */
+    /* step provides a lower bound, parl, for the zero of */
+    /* the function. otherwise set this bound to zero. */
+    parl = 0.;
+    if (rank==n) {
+      wa1 = qr.colsPermutation().inverse() *  diag.cwiseProduct(wa2)/dxnorm;
+      qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
+      temp = wa1.blueNorm();
+      parl = fp / m_delta / temp / temp;
+    }
+
+    /* calculate an upper bound, paru, for the zero of the function. */
+    for (j = 0; j < n; ++j)
+      wa1[j] = qr.matrixQR().col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)];
+
+    gnorm = wa1.stableNorm();
+    paru = gnorm / m_delta;
+    if (paru == 0.)
+      paru = dwarf / (std::min)(m_delta,Scalar(0.1));
+
+    /* if the input par lies outside of the interval (parl,paru), */
+    /* set par to the closer endpoint. */
+    par = (std::max)(par,parl);
+    par = (std::min)(par,paru);
+    if (par == 0.)
+      par = gnorm / dxnorm;
+
+    /* beginning of an iteration. */
+    MatrixType s;
+    s = qr.matrixQR();
+    while (true) {
+      ++iter;
+
+      /* evaluate the function at the current value of par. */
+      if (par == 0.)
+        par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
+      wa1 = sqrt(par)* diag;
+
+      VectorType sdiag(n);
+      lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag);
+
+      wa2 = diag.cwiseProduct(x);
+      dxnorm = wa2.blueNorm();
+      temp = fp;
+      fp = dxnorm - m_delta;
+
+      /* if the function is small enough, accept the current value */
+      /* of par. also test for the exceptional cases where parl */
+      /* is zero or the number of iterations has reached 10. */
+      if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
+        break;
+
+      /* 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().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
+      for (j = 0; j < n; ++j) {
+        wa1[j] /= sdiag[j];
+        temp = wa1[j];
+        for (Index i = j+1; i < n; ++i)
+          wa1[i] -= s.coeff(i,j) * temp;
+      }
+      temp = wa1.blueNorm();
+      parc = fp / m_delta / temp / temp;
+
+      /* depending on the sign of the function, update parl or paru. */
+      if (fp > 0.)
+        parl = (std::max)(parl,par);
+      if (fp < 0.)
+        paru = (std::min)(paru,par);
+
+      /* compute an improved estimate for par. */
+      par = (std::max)(parl,par+parc);
+    }
+    if (iter == 0)
+      par = 0.;
+    return;
+  }
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_LMPAR_H
diff --git a/Eigen/src/LevenbergMarquardt/LMqrsolv.h b/Eigen/src/LevenbergMarquardt/LMqrsolv.h
index 7a89306c0..ed6f97fe8 100644
--- a/Eigen/src/LevenbergMarquardt/LMqrsolv.h
+++ b/Eigen/src/LevenbergMarquardt/LMqrsolv.h
@@ -2,13 +2,19 @@
 // for linear algebra.
 //
 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
-// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr
+// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
 //
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+// 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 {
@@ -179,4 +185,5 @@ void lmqrsolv(
 } // end namespace internal
 
 } // end namespace Eigen
-#endif
\ No newline at end of file
+
+#endif // EIGEN_LMQRSOLV_H
diff --git a/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h b/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h
index f27aa7ddf..e45e73ab5 100644
--- a/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h
+++ b/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h
@@ -1,12 +1,17 @@
-// -*- coding: utf-8
-// vim: set fileencoding=utf-8
-
 // 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>
 //
+// The algorithm of this class 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.
+//
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
@@ -284,163 +289,6 @@ LevenbergMarquardt<FunctorType>::minimizeInit(FVectorType  &x)
 
 template<typename FunctorType>
 LevenbergMarquardtSpace::Status
-LevenbergMarquardt<FunctorType>::minimizeOneStep(FVectorType  &x)
-{
-  typedef typename FunctorType::JacobianType JacobianType; 
-  using std::abs;
-  using std::sqrt;
-  RealScalar temp, temp1,temp2; 
-  RealScalar ratio; 
-  RealScalar pnorm, xnorm, fnorm1, actred, dirder, prered;
-  assert(x.size()==n); // check the caller is not cheating us
-
-  temp = 0.0; xnorm = 0.0;
-  /* calculate the jacobian matrix. */
-  Index df_ret = m_functor.df(x, m_fjac);
-  if (df_ret<0)
-      return LevenbergMarquardtSpace::UserAsked;
-  if (df_ret>0)
-      // numerical diff, we evaluated the function df_ret times
-      m_nfev += df_ret;
-  else m_njev++;
-
-  /* compute the qr factorization of the jacobian. */
-  for (int j = 0; j < x.size(); ++j)
-    m_wa2(j) = m_fjac.col(j).norm(); 
-  //FIXME Implement bluenorm for sparse vectors
-//     m_wa2 = m_fjac.colwise().blueNorm();
-  QRSolver qrfac(m_fjac); //FIXME Check if the QR decomposition succeed
-  // Make a copy of the first factor with the associated permutation
-  JacobianType rfactor;
-  rfactor = qrfac.matrixQR();
-  m_permutation = (qrfac.colsPermutation());
-
-  /* on the first iteration and if external scaling is not used, scale according */
-  /* to the norms of the columns of the initial jacobian. */
-  if (m_iter == 1) {
-      if (!m_useExternalScaling)
-          for (Index j = 0; j < n; ++j)
-              m_diag[j] = (m_wa2[j]==0.)? 1. : m_wa2[j];
-
-      /* on the first iteration, calculate the norm of the scaled x */
-      /* and initialize the step bound m_delta. */
-      xnorm = m_diag.cwiseProduct(x).stableNorm();
-      m_delta = m_factor * xnorm;
-      if (m_delta == 0.)
-          m_delta = m_factor;
-  }
-
-  /* form (q transpose)*m_fvec and store the first n components in */
-  /* m_qtf. */
-  m_wa4 = m_fvec;
-  m_wa4 = qrfac.matrixQ().adjoint() * m_fvec; 
-  m_qtf = m_wa4.head(n);
-
-  /* compute the norm of the scaled gradient. */
-  m_gnorm = 0.;
-  if (m_fnorm != 0.)
-      for (Index j = 0; j < n; ++j)
-          if (m_wa2[m_permutation.indices()[j]] != 0.)
-              m_gnorm = (std::max)(m_gnorm, abs( rfactor.col(j).head(j+1).dot(m_qtf.head(j+1)/m_fnorm) / m_wa2[m_permutation.indices()[j]]));
-
-  /* test for convergence of the gradient norm. */
-  if (m_gnorm <= m_gtol)
-      return LevenbergMarquardtSpace::CosinusTooSmall;
-
-  /* rescale if necessary. */
-  if (!m_useExternalScaling)
-      m_diag = m_diag.cwiseMax(m_wa2);
-
-  do {
-    /* determine the levenberg-marquardt parameter. */
-    internal::lmpar2(qrfac, m_diag, m_qtf, m_delta, m_par, m_wa1);
-
-    /* store the direction p and x + p. calculate the norm of p. */
-    m_wa1 = -m_wa1;
-    m_wa2 = x + m_wa1;
-    pnorm = m_diag.cwiseProduct(m_wa1).stableNorm();
-
-    /* on the first iteration, adjust the initial step bound. */
-    if (m_iter == 1)
-        m_delta = (std::min)(m_delta,pnorm);
-
-    /* evaluate the function at x + p and calculate its norm. */
-    if ( m_functor(m_wa2, m_wa4) < 0)
-        return LevenbergMarquardtSpace::UserAsked;
-    ++m_nfev;
-    fnorm1 = m_wa4.stableNorm();
-
-    /* compute the scaled actual reduction. */
-    actred = -1.;
-    if (Scalar(.1) * fnorm1 < m_fnorm)
-        actred = 1. - internal::abs2(fnorm1 / m_fnorm);
-
-    /* compute the scaled predicted reduction and */
-    /* the scaled directional derivative. */
-    m_wa3 = rfactor.template triangularView<Upper>() * (m_permutation.inverse() *m_wa1);
-    temp1 = internal::abs2(m_wa3.stableNorm() / m_fnorm);
-    temp2 = internal::abs2(sqrt(m_par) * pnorm / m_fnorm);
-    prered = temp1 + temp2 / Scalar(.5);
-    dirder = -(temp1 + temp2);
-
-    /* compute the ratio of the actual to the predicted */
-    /* reduction. */
-    ratio = 0.;
-    if (prered != 0.)
-        ratio = actred / prered;
-
-    /* update the step bound. */
-    if (ratio <= Scalar(.25)) {
-        if (actred >= 0.)
-            temp = RealScalar(.5);
-        if (actred < 0.)
-            temp = RealScalar(.5) * dirder / (dirder + RealScalar(.5) * actred);
-        if (RealScalar(.1) * fnorm1 >= m_fnorm || temp < RealScalar(.1))
-            temp = Scalar(.1);
-        /* Computing MIN */
-        m_delta = temp * (std::min)(m_delta, pnorm / RealScalar(.1));
-        m_par /= temp;
-    } else if (!(m_par != 0. && ratio < RealScalar(.75))) {
-        m_delta = pnorm / RealScalar(.5);
-        m_par = RealScalar(.5) * m_par;
-    }
-
-    /* test for successful iteration. */
-    if (ratio >= RealScalar(1e-4)) {
-        /* successful iteration. update x, m_fvec, and their norms. */
-        x = m_wa2;
-        m_wa2 = m_diag.cwiseProduct(x);
-        m_fvec = m_wa4;
-        xnorm = m_wa2.stableNorm();
-        m_fnorm = fnorm1;
-        ++m_iter;
-    }
-
-    /* tests for convergence. */
-    if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1. && m_delta <= m_xtol * xnorm)
-        return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
-    if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1.)
-        return LevenbergMarquardtSpace::RelativeReductionTooSmall;
-    if (m_delta <= m_xtol * xnorm)
-        return LevenbergMarquardtSpace::RelativeErrorTooSmall;
-
-    /* tests for termination and stringent tolerances. */
-    if (m_nfev >= m_maxfev)
-        return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
-    if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
-        return LevenbergMarquardtSpace::FtolTooSmall;
-    if (m_delta <= NumTraits<Scalar>::epsilon() * xnorm)
-        return LevenbergMarquardtSpace::XtolTooSmall;
-    if (m_gnorm <= NumTraits<Scalar>::epsilon())
-        return LevenbergMarquardtSpace::GtolTooSmall;
-
-  } while (ratio < Scalar(1e-4));
-
-  return LevenbergMarquardtSpace::Running;
-}
-
-template<typename FunctorType>
-LevenbergMarquardtSpace::Status
 LevenbergMarquardt<FunctorType>::lmder1(
         FVectorType  &x,
         const Scalar tol
@@ -491,146 +339,6 @@ LevenbergMarquardt<FunctorType>::lmdif1(
     return info;
 }
 
-namespace internal {
-  
-  template <typename QRSolver, typename VectorType>
-    void lmpar2(
-		const QRSolver &qr,
-		const VectorType  &diag,
-		const VectorType  &qtb,
-		typename VectorType::Scalar m_delta,
-		typename VectorType::Scalar &par,
-		VectorType  &x)
-
-  {
-    using std::sqrt;
-    using std::abs;
-    typedef typename QRSolver::MatrixType MatrixType;
-    typedef typename QRSolver::Scalar Scalar;
-    typedef typename QRSolver::Index Index;
-
-    /* Local variables */
-    Index j;
-    Scalar fp;
-    Scalar parc, parl;
-    Index iter;
-    Scalar temp, paru;
-    Scalar gnorm;
-    Scalar dxnorm;
-
-
-    /* Function Body */
-    const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
-    const Index n = qr.matrixQR().cols();
-    assert(n==diag.size());
-    assert(n==qtb.size());
-
-    VectorType  wa1, wa2;
-
-    /* compute and store in x the gauss-newton direction. if the */
-    /* jacobian is rank-deficient, obtain a least squares solution. */
-
-    //    const Index rank = qr.nonzeroPivots(); // exactly double(0.)
-    const Index rank = qr.rank(); // use a threshold
-    wa1 = qtb;
-    wa1.tail(n-rank).setZero();
-    //FIXME There is no solve in place for sparse triangularView
-    //qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));
-    wa1.head(rank) = qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solve(qtb.head(rank));
-
-    x = qr.colsPermutation()*wa1;
-
-    /* initialize the iteration counter. */
-    /* evaluate the function at the origin, and test */
-    /* for acceptance of the gauss-newton direction. */
-    iter = 0;
-    wa2 = diag.cwiseProduct(x);
-    dxnorm = wa2.blueNorm();
-    fp = dxnorm - m_delta;
-    if (fp <= Scalar(0.1) * m_delta) {
-      par = 0;
-      return;
-    }
-
-    /* if the jacobian is not rank deficient, the newton */
-    /* step provides a lower bound, parl, for the zero of */
-    /* the function. otherwise set this bound to zero. */
-    parl = 0.;
-    if (rank==n) {
-      wa1 = qr.colsPermutation().inverse() *  diag.cwiseProduct(wa2)/dxnorm;
-      qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
-      temp = wa1.blueNorm();
-      parl = fp / m_delta / temp / temp;
-    }
-
-    /* calculate an upper bound, paru, for the zero of the function. */
-    for (j = 0; j < n; ++j)
-      wa1[j] = qr.matrixQR().col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)];
-
-    gnorm = wa1.stableNorm();
-    paru = gnorm / m_delta;
-    if (paru == 0.)
-      paru = dwarf / (std::min)(m_delta,Scalar(0.1));
-
-    /* if the input par lies outside of the interval (parl,paru), */
-    /* set par to the closer endpoint. */
-    par = (std::max)(par,parl);
-    par = (std::min)(par,paru);
-    if (par == 0.)
-      par = gnorm / dxnorm;
-
-    /* beginning of an iteration. */
-    MatrixType s;
-    s = qr.matrixQR();
-    while (true) {
-      ++iter;
-
-      /* evaluate the function at the current value of par. */
-      if (par == 0.)
-	par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
-      wa1 = sqrt(par)* diag;
-
-      VectorType sdiag(n);
-      lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag);
-
-      wa2 = diag.cwiseProduct(x);
-      dxnorm = wa2.blueNorm();
-      temp = fp;
-      fp = dxnorm - m_delta;
-
-      /* if the function is small enough, accept the current value */
-      /* of par. also test for the exceptional cases where parl */
-      /* is zero or the number of iterations has reached 10. */
-      if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
-	break;
-
-      /* 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().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
-      for (j = 0; j < n; ++j) {
-	wa1[j] /= sdiag[j];
-	temp = wa1[j];
-	for (Index i = j+1; i < n; ++i)
-	  wa1[i] -= s.coeff(i,j) * temp;
-      }
-      temp = wa1.blueNorm();
-      parc = fp / m_delta / temp / temp;
-
-      /* depending on the sign of the function, update parl or paru. */
-      if (fp > 0.)
-	parl = (std::max)(parl,par);
-      if (fp < 0.)
-	paru = (std::min)(paru,par);
-
-      /* compute an improved estimate for par. */
-      par = (std::max)(parl,par+parc);
-    }
-    if (iter == 0)
-      par = 0.;
-    return;
-  }
-} // end namespace internal
 } // end namespace Eigen
 
 #endif // EIGEN_LEVENBERGMARQUARDT_H