Move work in progress Levenberg Marquardt module in unsupported

1 files changed, 179 insertions, 0 deletions

diff --git a/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h b/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h
new file mode 100644
index 000000000..aa0c02668
--- /dev/null
+++ b/unsupported/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