Squashed 'third_party/eigen/' content from commit 61d72f6
Change-Id: Iccc90fa0b55ab44037f018046d2fcffd90d9d025
git-subtree-dir: third_party/eigen
git-subtree-split: 61d72f6383cfa842868c53e30e087b0258177257
diff --git a/unsupported/Eigen/src/NonLinearOptimization/CMakeLists.txt b/unsupported/Eigen/src/NonLinearOptimization/CMakeLists.txt
new file mode 100644
index 0000000..9322dda
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/CMakeLists.txt
@@ -0,0 +1,6 @@
+FILE(GLOB Eigen_NonLinearOptimization_SRCS "*.h")
+
+INSTALL(FILES
+ ${Eigen_NonLinearOptimization_SRCS}
+ DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/NonLinearOptimization COMPONENT Devel
+ )
diff --git a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
new file mode 100644
index 0000000..b8ba6dd
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
@@ -0,0 +1,601 @@
+// -*- 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>
+//
+// 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/.
+
+#ifndef EIGEN_HYBRIDNONLINEARSOLVER_H
+#define EIGEN_HYBRIDNONLINEARSOLVER_H
+
+namespace Eigen {
+
+namespace HybridNonLinearSolverSpace {
+ enum Status {
+ Running = -1,
+ ImproperInputParameters = 0,
+ RelativeErrorTooSmall = 1,
+ TooManyFunctionEvaluation = 2,
+ TolTooSmall = 3,
+ NotMakingProgressJacobian = 4,
+ NotMakingProgressIterations = 5,
+ UserAsked = 6
+ };
+}
+
+/**
+ * \ingroup NonLinearOptimization_Module
+ * \brief Finds a zero of a system of n
+ * nonlinear functions in n variables by a modification of the Powell
+ * hybrid method ("dogleg").
+ *
+ * The user must provide a subroutine which calculates the
+ * functions. The Jacobian is either provided by the user, or approximated
+ * using a forward-difference method.
+ *
+ */
+template<typename FunctorType, typename Scalar=double>
+class HybridNonLinearSolver
+{
+public:
+ typedef DenseIndex Index;
+
+ HybridNonLinearSolver(FunctorType &_functor)
+ : functor(_functor) { nfev=njev=iter = 0; fnorm= 0.; useExternalScaling=false;}
+
+ struct Parameters {
+ Parameters()
+ : factor(Scalar(100.))
+ , maxfev(1000)
+ , xtol(std::sqrt(NumTraits<Scalar>::epsilon()))
+ , nb_of_subdiagonals(-1)
+ , nb_of_superdiagonals(-1)
+ , epsfcn(Scalar(0.)) {}
+ Scalar factor;
+ Index maxfev; // maximum number of function evaluation
+ Scalar xtol;
+ Index nb_of_subdiagonals;
+ Index nb_of_superdiagonals;
+ Scalar epsfcn;
+ };
+ typedef Matrix< Scalar, Dynamic, 1 > FVectorType;
+ typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType;
+ /* TODO: if eigen provides a triangular storage, use it here */
+ typedef Matrix< Scalar, Dynamic, Dynamic > UpperTriangularType;
+
+ HybridNonLinearSolverSpace::Status hybrj1(
+ FVectorType &x,
+ const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
+ );
+
+ HybridNonLinearSolverSpace::Status solveInit(FVectorType &x);
+ HybridNonLinearSolverSpace::Status solveOneStep(FVectorType &x);
+ HybridNonLinearSolverSpace::Status solve(FVectorType &x);
+
+ HybridNonLinearSolverSpace::Status hybrd1(
+ FVectorType &x,
+ const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
+ );
+
+ HybridNonLinearSolverSpace::Status solveNumericalDiffInit(FVectorType &x);
+ HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(FVectorType &x);
+ HybridNonLinearSolverSpace::Status solveNumericalDiff(FVectorType &x);
+
+ void resetParameters(void) { parameters = Parameters(); }
+ Parameters parameters;
+ FVectorType fvec, qtf, diag;
+ JacobianType fjac;
+ UpperTriangularType R;
+ Index nfev;
+ Index njev;
+ Index iter;
+ Scalar fnorm;
+ bool useExternalScaling;
+private:
+ FunctorType &functor;
+ Index n;
+ Scalar sum;
+ bool sing;
+ Scalar temp;
+ Scalar delta;
+ bool jeval;
+ Index ncsuc;
+ Scalar ratio;
+ Scalar pnorm, xnorm, fnorm1;
+ Index nslow1, nslow2;
+ Index ncfail;
+ Scalar actred, prered;
+ FVectorType wa1, wa2, wa3, wa4;
+
+ HybridNonLinearSolver& operator=(const HybridNonLinearSolver&);
+};
+
+
+
+template<typename FunctorType, typename Scalar>
+HybridNonLinearSolverSpace::Status
+HybridNonLinearSolver<FunctorType,Scalar>::hybrj1(
+ FVectorType &x,
+ const Scalar tol
+ )
+{
+ n = x.size();
+
+ /* check the input parameters for errors. */
+ if (n <= 0 || tol < 0.)
+ return HybridNonLinearSolverSpace::ImproperInputParameters;
+
+ resetParameters();
+ parameters.maxfev = 100*(n+1);
+ parameters.xtol = tol;
+ diag.setConstant(n, 1.);
+ useExternalScaling = true;
+ return solve(x);
+}
+
+template<typename FunctorType, typename Scalar>
+HybridNonLinearSolverSpace::Status
+HybridNonLinearSolver<FunctorType,Scalar>::solveInit(FVectorType &x)
+{
+ n = x.size();
+
+ wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n);
+ fvec.resize(n);
+ qtf.resize(n);
+ fjac.resize(n, n);
+ if (!useExternalScaling)
+ diag.resize(n);
+ eigen_assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'");
+
+ /* Function Body */
+ nfev = 0;
+ njev = 0;
+
+ /* check the input parameters for errors. */
+ if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0. )
+ return HybridNonLinearSolverSpace::ImproperInputParameters;
+ if (useExternalScaling)
+ for (Index j = 0; j < n; ++j)
+ if (diag[j] <= 0.)
+ return HybridNonLinearSolverSpace::ImproperInputParameters;
+
+ /* evaluate the function at the starting point */
+ /* and calculate its norm. */
+ nfev = 1;
+ if ( functor(x, fvec) < 0)
+ return HybridNonLinearSolverSpace::UserAsked;
+ fnorm = fvec.stableNorm();
+
+ /* initialize iteration counter and monitors. */
+ iter = 1;
+ ncsuc = 0;
+ ncfail = 0;
+ nslow1 = 0;
+ nslow2 = 0;
+
+ return HybridNonLinearSolverSpace::Running;
+}
+
+template<typename FunctorType, typename Scalar>
+HybridNonLinearSolverSpace::Status
+HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(FVectorType &x)
+{
+ using std::abs;
+
+ eigen_assert(x.size()==n); // check the caller is not cheating us
+
+ Index j;
+ std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
+
+ jeval = true;
+
+ /* calculate the jacobian matrix. */
+ if ( functor.df(x, fjac) < 0)
+ return HybridNonLinearSolverSpace::UserAsked;
+ ++njev;
+
+ wa2 = fjac.colwise().blueNorm();
+
+ /* on the first iteration and if external scaling is not used, scale according */
+ /* to the norms of the columns of the initial jacobian. */
+ if (iter == 1) {
+ if (!useExternalScaling)
+ for (j = 0; j < n; ++j)
+ diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
+
+ /* on the first iteration, calculate the norm of the scaled x */
+ /* and initialize the step bound delta. */
+ xnorm = diag.cwiseProduct(x).stableNorm();
+ delta = parameters.factor * xnorm;
+ if (delta == 0.)
+ delta = parameters.factor;
+ }
+
+ /* compute the qr factorization of the jacobian. */
+ HouseholderQR<JacobianType> qrfac(fjac); // no pivoting:
+
+ /* copy the triangular factor of the qr factorization into r. */
+ R = qrfac.matrixQR();
+
+ /* accumulate the orthogonal factor in fjac. */
+ fjac = qrfac.householderQ();
+
+ /* form (q transpose)*fvec and store in qtf. */
+ qtf = fjac.transpose() * fvec;
+
+ /* rescale if necessary. */
+ if (!useExternalScaling)
+ diag = diag.cwiseMax(wa2);
+
+ while (true) {
+ /* determine the direction p. */
+ internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
+
+ /* store the direction p and x + p. calculate the norm of p. */
+ wa1 = -wa1;
+ wa2 = x + wa1;
+ pnorm = diag.cwiseProduct(wa1).stableNorm();
+
+ /* on the first iteration, adjust the initial step bound. */
+ if (iter == 1)
+ delta = (std::min)(delta,pnorm);
+
+ /* evaluate the function at x + p and calculate its norm. */
+ if ( functor(wa2, wa4) < 0)
+ return HybridNonLinearSolverSpace::UserAsked;
+ ++nfev;
+ fnorm1 = wa4.stableNorm();
+
+ /* compute the scaled actual reduction. */
+ actred = -1.;
+ if (fnorm1 < fnorm) /* Computing 2nd power */
+ actred = 1. - numext::abs2(fnorm1 / fnorm);
+
+ /* compute the scaled predicted reduction. */
+ wa3 = R.template triangularView<Upper>()*wa1 + qtf;
+ temp = wa3.stableNorm();
+ prered = 0.;
+ if (temp < fnorm) /* Computing 2nd power */
+ prered = 1. - numext::abs2(temp / fnorm);
+
+ /* 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(.1)) {
+ ncsuc = 0;
+ ++ncfail;
+ delta = Scalar(.5) * delta;
+ } else {
+ ncfail = 0;
+ ++ncsuc;
+ if (ratio >= Scalar(.5) || ncsuc > 1)
+ delta = (std::max)(delta, pnorm / Scalar(.5));
+ if (abs(ratio - 1.) <= Scalar(.1)) {
+ delta = pnorm / Scalar(.5);
+ }
+ }
+
+ /* test for successful iteration. */
+ if (ratio >= Scalar(1e-4)) {
+ /* successful iteration. update x, fvec, and their norms. */
+ x = wa2;
+ wa2 = diag.cwiseProduct(x);
+ fvec = wa4;
+ xnorm = wa2.stableNorm();
+ fnorm = fnorm1;
+ ++iter;
+ }
+
+ /* determine the progress of the iteration. */
+ ++nslow1;
+ if (actred >= Scalar(.001))
+ nslow1 = 0;
+ if (jeval)
+ ++nslow2;
+ if (actred >= Scalar(.1))
+ nslow2 = 0;
+
+ /* test for convergence. */
+ if (delta <= parameters.xtol * xnorm || fnorm == 0.)
+ return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
+
+ /* tests for termination and stringent tolerances. */
+ if (nfev >= parameters.maxfev)
+ return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
+ if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
+ return HybridNonLinearSolverSpace::TolTooSmall;
+ if (nslow2 == 5)
+ return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
+ if (nslow1 == 10)
+ return HybridNonLinearSolverSpace::NotMakingProgressIterations;
+
+ /* criterion for recalculating jacobian. */
+ if (ncfail == 2)
+ break; // leave inner loop and go for the next outer loop iteration
+
+ /* calculate the rank one modification to the jacobian */
+ /* and update qtf if necessary. */
+ wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
+ wa2 = fjac.transpose() * wa4;
+ if (ratio >= Scalar(1e-4))
+ qtf = wa2;
+ wa2 = (wa2-wa3)/pnorm;
+
+ /* compute the qr factorization of the updated jacobian. */
+ internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
+ internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
+ internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
+
+ jeval = false;
+ }
+ return HybridNonLinearSolverSpace::Running;
+}
+
+template<typename FunctorType, typename Scalar>
+HybridNonLinearSolverSpace::Status
+HybridNonLinearSolver<FunctorType,Scalar>::solve(FVectorType &x)
+{
+ HybridNonLinearSolverSpace::Status status = solveInit(x);
+ if (status==HybridNonLinearSolverSpace::ImproperInputParameters)
+ return status;
+ while (status==HybridNonLinearSolverSpace::Running)
+ status = solveOneStep(x);
+ return status;
+}
+
+
+
+template<typename FunctorType, typename Scalar>
+HybridNonLinearSolverSpace::Status
+HybridNonLinearSolver<FunctorType,Scalar>::hybrd1(
+ FVectorType &x,
+ const Scalar tol
+ )
+{
+ n = x.size();
+
+ /* check the input parameters for errors. */
+ if (n <= 0 || tol < 0.)
+ return HybridNonLinearSolverSpace::ImproperInputParameters;
+
+ resetParameters();
+ parameters.maxfev = 200*(n+1);
+ parameters.xtol = tol;
+
+ diag.setConstant(n, 1.);
+ useExternalScaling = true;
+ return solveNumericalDiff(x);
+}
+
+template<typename FunctorType, typename Scalar>
+HybridNonLinearSolverSpace::Status
+HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(FVectorType &x)
+{
+ n = x.size();
+
+ if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
+ if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
+
+ wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n);
+ qtf.resize(n);
+ fjac.resize(n, n);
+ fvec.resize(n);
+ if (!useExternalScaling)
+ diag.resize(n);
+ eigen_assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'");
+
+ /* Function Body */
+ nfev = 0;
+ njev = 0;
+
+ /* check the input parameters for errors. */
+ if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. )
+ return HybridNonLinearSolverSpace::ImproperInputParameters;
+ if (useExternalScaling)
+ for (Index j = 0; j < n; ++j)
+ if (diag[j] <= 0.)
+ return HybridNonLinearSolverSpace::ImproperInputParameters;
+
+ /* evaluate the function at the starting point */
+ /* and calculate its norm. */
+ nfev = 1;
+ if ( functor(x, fvec) < 0)
+ return HybridNonLinearSolverSpace::UserAsked;
+ fnorm = fvec.stableNorm();
+
+ /* initialize iteration counter and monitors. */
+ iter = 1;
+ ncsuc = 0;
+ ncfail = 0;
+ nslow1 = 0;
+ nslow2 = 0;
+
+ return HybridNonLinearSolverSpace::Running;
+}
+
+template<typename FunctorType, typename Scalar>
+HybridNonLinearSolverSpace::Status
+HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(FVectorType &x)
+{
+ using std::sqrt;
+ using std::abs;
+
+ assert(x.size()==n); // check the caller is not cheating us
+
+ Index j;
+ std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
+
+ jeval = true;
+ if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
+ if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
+
+ /* calculate the jacobian matrix. */
+ if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals, parameters.epsfcn) <0)
+ return HybridNonLinearSolverSpace::UserAsked;
+ nfev += (std::min)(parameters.nb_of_subdiagonals+parameters.nb_of_superdiagonals+ 1, n);
+
+ wa2 = fjac.colwise().blueNorm();
+
+ /* on the first iteration and if external scaling is not used, scale according */
+ /* to the norms of the columns of the initial jacobian. */
+ if (iter == 1) {
+ if (!useExternalScaling)
+ for (j = 0; j < n; ++j)
+ diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
+
+ /* on the first iteration, calculate the norm of the scaled x */
+ /* and initialize the step bound delta. */
+ xnorm = diag.cwiseProduct(x).stableNorm();
+ delta = parameters.factor * xnorm;
+ if (delta == 0.)
+ delta = parameters.factor;
+ }
+
+ /* compute the qr factorization of the jacobian. */
+ HouseholderQR<JacobianType> qrfac(fjac); // no pivoting:
+
+ /* copy the triangular factor of the qr factorization into r. */
+ R = qrfac.matrixQR();
+
+ /* accumulate the orthogonal factor in fjac. */
+ fjac = qrfac.householderQ();
+
+ /* form (q transpose)*fvec and store in qtf. */
+ qtf = fjac.transpose() * fvec;
+
+ /* rescale if necessary. */
+ if (!useExternalScaling)
+ diag = diag.cwiseMax(wa2);
+
+ while (true) {
+ /* determine the direction p. */
+ internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
+
+ /* store the direction p and x + p. calculate the norm of p. */
+ wa1 = -wa1;
+ wa2 = x + wa1;
+ pnorm = diag.cwiseProduct(wa1).stableNorm();
+
+ /* on the first iteration, adjust the initial step bound. */
+ if (iter == 1)
+ delta = (std::min)(delta,pnorm);
+
+ /* evaluate the function at x + p and calculate its norm. */
+ if ( functor(wa2, wa4) < 0)
+ return HybridNonLinearSolverSpace::UserAsked;
+ ++nfev;
+ fnorm1 = wa4.stableNorm();
+
+ /* compute the scaled actual reduction. */
+ actred = -1.;
+ if (fnorm1 < fnorm) /* Computing 2nd power */
+ actred = 1. - numext::abs2(fnorm1 / fnorm);
+
+ /* compute the scaled predicted reduction. */
+ wa3 = R.template triangularView<Upper>()*wa1 + qtf;
+ temp = wa3.stableNorm();
+ prered = 0.;
+ if (temp < fnorm) /* Computing 2nd power */
+ prered = 1. - numext::abs2(temp / fnorm);
+
+ /* 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(.1)) {
+ ncsuc = 0;
+ ++ncfail;
+ delta = Scalar(.5) * delta;
+ } else {
+ ncfail = 0;
+ ++ncsuc;
+ if (ratio >= Scalar(.5) || ncsuc > 1)
+ delta = (std::max)(delta, pnorm / Scalar(.5));
+ if (abs(ratio - 1.) <= Scalar(.1)) {
+ delta = pnorm / Scalar(.5);
+ }
+ }
+
+ /* test for successful iteration. */
+ if (ratio >= Scalar(1e-4)) {
+ /* successful iteration. update x, fvec, and their norms. */
+ x = wa2;
+ wa2 = diag.cwiseProduct(x);
+ fvec = wa4;
+ xnorm = wa2.stableNorm();
+ fnorm = fnorm1;
+ ++iter;
+ }
+
+ /* determine the progress of the iteration. */
+ ++nslow1;
+ if (actred >= Scalar(.001))
+ nslow1 = 0;
+ if (jeval)
+ ++nslow2;
+ if (actred >= Scalar(.1))
+ nslow2 = 0;
+
+ /* test for convergence. */
+ if (delta <= parameters.xtol * xnorm || fnorm == 0.)
+ return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
+
+ /* tests for termination and stringent tolerances. */
+ if (nfev >= parameters.maxfev)
+ return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
+ if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
+ return HybridNonLinearSolverSpace::TolTooSmall;
+ if (nslow2 == 5)
+ return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
+ if (nslow1 == 10)
+ return HybridNonLinearSolverSpace::NotMakingProgressIterations;
+
+ /* criterion for recalculating jacobian. */
+ if (ncfail == 2)
+ break; // leave inner loop and go for the next outer loop iteration
+
+ /* calculate the rank one modification to the jacobian */
+ /* and update qtf if necessary. */
+ wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
+ wa2 = fjac.transpose() * wa4;
+ if (ratio >= Scalar(1e-4))
+ qtf = wa2;
+ wa2 = (wa2-wa3)/pnorm;
+
+ /* compute the qr factorization of the updated jacobian. */
+ internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
+ internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
+ internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
+
+ jeval = false;
+ }
+ return HybridNonLinearSolverSpace::Running;
+}
+
+template<typename FunctorType, typename Scalar>
+HybridNonLinearSolverSpace::Status
+HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(FVectorType &x)
+{
+ HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x);
+ if (status==HybridNonLinearSolverSpace::ImproperInputParameters)
+ return status;
+ while (status==HybridNonLinearSolverSpace::Running)
+ status = solveNumericalDiffOneStep(x);
+ return status;
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_HYBRIDNONLINEARSOLVER_H
+
+//vim: ai ts=4 sts=4 et sw=4
diff --git a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
new file mode 100644
index 0000000..bfeb26f
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
@@ -0,0 +1,650 @@
+// -*- 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>
+//
+// 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/.
+
+#ifndef EIGEN_LEVENBERGMARQUARDT__H
+#define EIGEN_LEVENBERGMARQUARDT__H
+
+namespace Eigen {
+
+namespace LevenbergMarquardtSpace {
+ enum Status {
+ NotStarted = -2,
+ Running = -1,
+ ImproperInputParameters = 0,
+ RelativeReductionTooSmall = 1,
+ RelativeErrorTooSmall = 2,
+ RelativeErrorAndReductionTooSmall = 3,
+ CosinusTooSmall = 4,
+ TooManyFunctionEvaluation = 5,
+ FtolTooSmall = 6,
+ XtolTooSmall = 7,
+ GtolTooSmall = 8,
+ UserAsked = 9
+ };
+}
+
+
+
+/**
+ * \ingroup NonLinearOptimization_Module
+ * \brief Performs non linear optimization over a non-linear function,
+ * using a variant of the Levenberg Marquardt algorithm.
+ *
+ * Check wikipedia for more information.
+ * http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
+ */
+template<typename FunctorType, typename Scalar=double>
+class LevenbergMarquardt
+{
+public:
+ LevenbergMarquardt(FunctorType &_functor)
+ : functor(_functor) { nfev = njev = iter = 0; fnorm = gnorm = 0.; useExternalScaling=false; }
+
+ typedef DenseIndex Index;
+
+ struct Parameters {
+ Parameters()
+ : factor(Scalar(100.))
+ , maxfev(400)
+ , ftol(std::sqrt(NumTraits<Scalar>::epsilon()))
+ , xtol(std::sqrt(NumTraits<Scalar>::epsilon()))
+ , gtol(Scalar(0.))
+ , epsfcn(Scalar(0.)) {}
+ Scalar factor;
+ Index maxfev; // maximum number of function evaluation
+ Scalar ftol;
+ Scalar xtol;
+ Scalar gtol;
+ Scalar epsfcn;
+ };
+
+ typedef Matrix< Scalar, Dynamic, 1 > FVectorType;
+ typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType;
+
+ LevenbergMarquardtSpace::Status lmder1(
+ FVectorType &x,
+ const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
+ );
+
+ LevenbergMarquardtSpace::Status minimize(FVectorType &x);
+ LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x);
+ LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x);
+
+ static LevenbergMarquardtSpace::Status lmdif1(
+ FunctorType &functor,
+ FVectorType &x,
+ Index *nfev,
+ const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
+ );
+
+ LevenbergMarquardtSpace::Status lmstr1(
+ FVectorType &x,
+ const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
+ );
+
+ LevenbergMarquardtSpace::Status minimizeOptimumStorage(FVectorType &x);
+ LevenbergMarquardtSpace::Status minimizeOptimumStorageInit(FVectorType &x);
+ LevenbergMarquardtSpace::Status minimizeOptimumStorageOneStep(FVectorType &x);
+
+ void resetParameters(void) { parameters = Parameters(); }
+
+ Parameters parameters;
+ FVectorType fvec, qtf, diag;
+ JacobianType fjac;
+ PermutationMatrix<Dynamic,Dynamic> permutation;
+ Index nfev;
+ Index njev;
+ Index iter;
+ Scalar fnorm, gnorm;
+ bool useExternalScaling;
+
+ Scalar lm_param(void) { return par; }
+private:
+ FunctorType &functor;
+ Index n;
+ Index m;
+ FVectorType wa1, wa2, wa3, wa4;
+
+ Scalar par, sum;
+ Scalar temp, temp1, temp2;
+ Scalar delta;
+ Scalar ratio;
+ Scalar pnorm, xnorm, fnorm1, actred, dirder, prered;
+
+ LevenbergMarquardt& operator=(const LevenbergMarquardt&);
+};
+
+template<typename FunctorType, typename Scalar>
+LevenbergMarquardtSpace::Status
+LevenbergMarquardt<FunctorType,Scalar>::lmder1(
+ FVectorType &x,
+ const Scalar tol
+ )
+{
+ n = x.size();
+ m = functor.values();
+
+ /* check the input parameters for errors. */
+ if (n <= 0 || m < n || tol < 0.)
+ return LevenbergMarquardtSpace::ImproperInputParameters;
+
+ resetParameters();
+ parameters.ftol = tol;
+ parameters.xtol = tol;
+ parameters.maxfev = 100*(n+1);
+
+ return minimize(x);
+}
+
+
+template<typename FunctorType, typename Scalar>
+LevenbergMarquardtSpace::Status
+LevenbergMarquardt<FunctorType,Scalar>::minimize(FVectorType &x)
+{
+ LevenbergMarquardtSpace::Status status = minimizeInit(x);
+ if (status==LevenbergMarquardtSpace::ImproperInputParameters)
+ return status;
+ do {
+ status = minimizeOneStep(x);
+ } while (status==LevenbergMarquardtSpace::Running);
+ return status;
+}
+
+template<typename FunctorType, typename Scalar>
+LevenbergMarquardtSpace::Status
+LevenbergMarquardt<FunctorType,Scalar>::minimizeInit(FVectorType &x)
+{
+ n = x.size();
+ m = functor.values();
+
+ wa1.resize(n); wa2.resize(n); wa3.resize(n);
+ wa4.resize(m);
+ fvec.resize(m);
+ fjac.resize(m, n);
+ if (!useExternalScaling)
+ diag.resize(n);
+ eigen_assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'");
+ qtf.resize(n);
+
+ /* Function Body */
+ nfev = 0;
+ njev = 0;
+
+ /* check the input parameters for errors. */
+ if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
+ return LevenbergMarquardtSpace::ImproperInputParameters;
+
+ if (useExternalScaling)
+ for (Index j = 0; j < n; ++j)
+ if (diag[j] <= 0.)
+ return LevenbergMarquardtSpace::ImproperInputParameters;
+
+ /* evaluate the function at the starting point */
+ /* and calculate its norm. */
+ nfev = 1;
+ if ( functor(x, fvec) < 0)
+ return LevenbergMarquardtSpace::UserAsked;
+ fnorm = fvec.stableNorm();
+
+ /* initialize levenberg-marquardt parameter and iteration counter. */
+ par = 0.;
+ iter = 1;
+
+ return LevenbergMarquardtSpace::NotStarted;
+}
+
+template<typename FunctorType, typename Scalar>
+LevenbergMarquardtSpace::Status
+LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(FVectorType &x)
+{
+ using std::abs;
+ using std::sqrt;
+
+ eigen_assert(x.size()==n); // check the caller is not cheating us
+
+ /* calculate the jacobian matrix. */
+ Index df_ret = functor.df(x, fjac);
+ if (df_ret<0)
+ return LevenbergMarquardtSpace::UserAsked;
+ if (df_ret>0)
+ // numerical diff, we evaluated the function df_ret times
+ nfev += df_ret;
+ else njev++;
+
+ /* compute the qr factorization of the jacobian. */
+ wa2 = fjac.colwise().blueNorm();
+ ColPivHouseholderQR<JacobianType> qrfac(fjac);
+ fjac = qrfac.matrixQR();
+ 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 (iter == 1) {
+ if (!useExternalScaling)
+ for (Index j = 0; j < n; ++j)
+ diag[j] = (wa2[j]==0.)? 1. : wa2[j];
+
+ /* on the first iteration, calculate the norm of the scaled x */
+ /* and initialize the step bound delta. */
+ xnorm = diag.cwiseProduct(x).stableNorm();
+ delta = parameters.factor * xnorm;
+ if (delta == 0.)
+ delta = parameters.factor;
+ }
+
+ /* form (q transpose)*fvec and store the first n components in */
+ /* qtf. */
+ wa4 = fvec;
+ wa4.applyOnTheLeft(qrfac.householderQ().adjoint());
+ qtf = wa4.head(n);
+
+ /* compute the norm of the scaled gradient. */
+ gnorm = 0.;
+ if (fnorm != 0.)
+ for (Index j = 0; j < n; ++j)
+ if (wa2[permutation.indices()[j]] != 0.)
+ gnorm = (std::max)(gnorm, abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]]));
+
+ /* test for convergence of the gradient norm. */
+ if (gnorm <= parameters.gtol)
+ return LevenbergMarquardtSpace::CosinusTooSmall;
+
+ /* rescale if necessary. */
+ if (!useExternalScaling)
+ diag = diag.cwiseMax(wa2);
+
+ do {
+
+ /* determine the levenberg-marquardt parameter. */
+ internal::lmpar2<Scalar>(qrfac, diag, qtf, delta, par, wa1);
+
+ /* store the direction p and x + p. calculate the norm of p. */
+ wa1 = -wa1;
+ wa2 = x + wa1;
+ pnorm = diag.cwiseProduct(wa1).stableNorm();
+
+ /* on the first iteration, adjust the initial step bound. */
+ if (iter == 1)
+ delta = (std::min)(delta,pnorm);
+
+ /* evaluate the function at x + p and calculate its norm. */
+ if ( functor(wa2, wa4) < 0)
+ return LevenbergMarquardtSpace::UserAsked;
+ ++nfev;
+ fnorm1 = wa4.stableNorm();
+
+ /* compute the scaled actual reduction. */
+ actred = -1.;
+ if (Scalar(.1) * fnorm1 < fnorm)
+ actred = 1. - numext::abs2(fnorm1 / fnorm);
+
+ /* compute the scaled predicted reduction and */
+ /* the scaled directional derivative. */
+ wa3 = fjac.template triangularView<Upper>() * (qrfac.colsPermutation().inverse() *wa1);
+ temp1 = numext::abs2(wa3.stableNorm() / fnorm);
+ temp2 = numext::abs2(sqrt(par) * pnorm / 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 = Scalar(.5);
+ if (actred < 0.)
+ temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
+ if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
+ temp = Scalar(.1);
+ /* Computing MIN */
+ delta = temp * (std::min)(delta, pnorm / Scalar(.1));
+ par /= temp;
+ } else if (!(par != 0. && ratio < Scalar(.75))) {
+ delta = pnorm / Scalar(.5);
+ par = Scalar(.5) * par;
+ }
+
+ /* test for successful iteration. */
+ if (ratio >= Scalar(1e-4)) {
+ /* successful iteration. update x, fvec, and their norms. */
+ x = wa2;
+ wa2 = diag.cwiseProduct(x);
+ fvec = wa4;
+ xnorm = wa2.stableNorm();
+ fnorm = fnorm1;
+ ++iter;
+ }
+
+ /* tests for convergence. */
+ if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
+ return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
+ if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
+ return LevenbergMarquardtSpace::RelativeReductionTooSmall;
+ if (delta <= parameters.xtol * xnorm)
+ return LevenbergMarquardtSpace::RelativeErrorTooSmall;
+
+ /* tests for termination and stringent tolerances. */
+ if (nfev >= parameters.maxfev)
+ return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
+ if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
+ return LevenbergMarquardtSpace::FtolTooSmall;
+ if (delta <= NumTraits<Scalar>::epsilon() * xnorm)
+ return LevenbergMarquardtSpace::XtolTooSmall;
+ if (gnorm <= NumTraits<Scalar>::epsilon())
+ return LevenbergMarquardtSpace::GtolTooSmall;
+
+ } while (ratio < Scalar(1e-4));
+
+ return LevenbergMarquardtSpace::Running;
+}
+
+template<typename FunctorType, typename Scalar>
+LevenbergMarquardtSpace::Status
+LevenbergMarquardt<FunctorType,Scalar>::lmstr1(
+ FVectorType &x,
+ const Scalar tol
+ )
+{
+ n = x.size();
+ m = functor.values();
+
+ /* check the input parameters for errors. */
+ if (n <= 0 || m < n || tol < 0.)
+ return LevenbergMarquardtSpace::ImproperInputParameters;
+
+ resetParameters();
+ parameters.ftol = tol;
+ parameters.xtol = tol;
+ parameters.maxfev = 100*(n+1);
+
+ return minimizeOptimumStorage(x);
+}
+
+template<typename FunctorType, typename Scalar>
+LevenbergMarquardtSpace::Status
+LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageInit(FVectorType &x)
+{
+ n = x.size();
+ m = functor.values();
+
+ wa1.resize(n); wa2.resize(n); wa3.resize(n);
+ wa4.resize(m);
+ fvec.resize(m);
+ // Only R is stored in fjac. Q is only used to compute 'qtf', which is
+ // Q.transpose()*rhs. qtf will be updated using givens rotation,
+ // instead of storing them in Q.
+ // The purpose it to only use a nxn matrix, instead of mxn here, so
+ // that we can handle cases where m>>n :
+ fjac.resize(n, n);
+ if (!useExternalScaling)
+ diag.resize(n);
+ eigen_assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'");
+ qtf.resize(n);
+
+ /* Function Body */
+ nfev = 0;
+ njev = 0;
+
+ /* check the input parameters for errors. */
+ if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
+ return LevenbergMarquardtSpace::ImproperInputParameters;
+
+ if (useExternalScaling)
+ for (Index j = 0; j < n; ++j)
+ if (diag[j] <= 0.)
+ return LevenbergMarquardtSpace::ImproperInputParameters;
+
+ /* evaluate the function at the starting point */
+ /* and calculate its norm. */
+ nfev = 1;
+ if ( functor(x, fvec) < 0)
+ return LevenbergMarquardtSpace::UserAsked;
+ fnorm = fvec.stableNorm();
+
+ /* initialize levenberg-marquardt parameter and iteration counter. */
+ par = 0.;
+ iter = 1;
+
+ return LevenbergMarquardtSpace::NotStarted;
+}
+
+
+template<typename FunctorType, typename Scalar>
+LevenbergMarquardtSpace::Status
+LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(FVectorType &x)
+{
+ using std::abs;
+ using std::sqrt;
+
+ eigen_assert(x.size()==n); // check the caller is not cheating us
+
+ Index i, j;
+ bool sing;
+
+ /* compute the qr factorization of the jacobian matrix */
+ /* calculated one row at a time, while simultaneously */
+ /* forming (q transpose)*fvec and storing the first */
+ /* n components in qtf. */
+ qtf.fill(0.);
+ fjac.fill(0.);
+ Index rownb = 2;
+ for (i = 0; i < m; ++i) {
+ if (functor.df(x, wa3, rownb) < 0) return LevenbergMarquardtSpace::UserAsked;
+ internal::rwupdt<Scalar>(fjac, wa3, qtf, fvec[i]);
+ ++rownb;
+ }
+ ++njev;
+
+ /* if the jacobian is rank deficient, call qrfac to */
+ /* reorder its columns and update the components of qtf. */
+ sing = false;
+ for (j = 0; j < n; ++j) {
+ if (fjac(j,j) == 0.)
+ sing = true;
+ wa2[j] = fjac.col(j).head(j).stableNorm();
+ }
+ permutation.setIdentity(n);
+ if (sing) {
+ wa2 = fjac.colwise().blueNorm();
+ // TODO We have no unit test covering this code path, do not modify
+ // until it is carefully tested
+ ColPivHouseholderQR<JacobianType> qrfac(fjac);
+ fjac = qrfac.matrixQR();
+ wa1 = fjac.diagonal();
+ fjac.diagonal() = qrfac.hCoeffs();
+ permutation = qrfac.colsPermutation();
+ // TODO : avoid this:
+ for(Index ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors
+
+ for (j = 0; j < n; ++j) {
+ if (fjac(j,j) != 0.) {
+ sum = 0.;
+ for (i = j; i < n; ++i)
+ sum += fjac(i,j) * qtf[i];
+ temp = -sum / fjac(j,j);
+ for (i = j; i < n; ++i)
+ qtf[i] += fjac(i,j) * temp;
+ }
+ fjac(j,j) = wa1[j];
+ }
+ }
+
+ /* on the first iteration and if external scaling is not used, scale according */
+ /* to the norms of the columns of the initial jacobian. */
+ if (iter == 1) {
+ if (!useExternalScaling)
+ for (j = 0; j < n; ++j)
+ diag[j] = (wa2[j]==0.)? 1. : wa2[j];
+
+ /* on the first iteration, calculate the norm of the scaled x */
+ /* and initialize the step bound delta. */
+ xnorm = diag.cwiseProduct(x).stableNorm();
+ delta = parameters.factor * xnorm;
+ if (delta == 0.)
+ delta = parameters.factor;
+ }
+
+ /* compute the norm of the scaled gradient. */
+ gnorm = 0.;
+ if (fnorm != 0.)
+ for (j = 0; j < n; ++j)
+ if (wa2[permutation.indices()[j]] != 0.)
+ gnorm = (std::max)(gnorm, abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]]));
+
+ /* test for convergence of the gradient norm. */
+ if (gnorm <= parameters.gtol)
+ return LevenbergMarquardtSpace::CosinusTooSmall;
+
+ /* rescale if necessary. */
+ if (!useExternalScaling)
+ diag = diag.cwiseMax(wa2);
+
+ do {
+
+ /* determine the levenberg-marquardt parameter. */
+ internal::lmpar<Scalar>(fjac, permutation.indices(), diag, qtf, delta, par, wa1);
+
+ /* store the direction p and x + p. calculate the norm of p. */
+ wa1 = -wa1;
+ wa2 = x + wa1;
+ pnorm = diag.cwiseProduct(wa1).stableNorm();
+
+ /* on the first iteration, adjust the initial step bound. */
+ if (iter == 1)
+ delta = (std::min)(delta,pnorm);
+
+ /* evaluate the function at x + p and calculate its norm. */
+ if ( functor(wa2, wa4) < 0)
+ return LevenbergMarquardtSpace::UserAsked;
+ ++nfev;
+ fnorm1 = wa4.stableNorm();
+
+ /* compute the scaled actual reduction. */
+ actred = -1.;
+ if (Scalar(.1) * fnorm1 < fnorm)
+ actred = 1. - numext::abs2(fnorm1 / fnorm);
+
+ /* compute the scaled predicted reduction and */
+ /* the scaled directional derivative. */
+ wa3 = fjac.topLeftCorner(n,n).template triangularView<Upper>() * (permutation.inverse() * wa1);
+ temp1 = numext::abs2(wa3.stableNorm() / fnorm);
+ temp2 = numext::abs2(sqrt(par) * pnorm / 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 = Scalar(.5);
+ if (actred < 0.)
+ temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
+ if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
+ temp = Scalar(.1);
+ /* Computing MIN */
+ delta = temp * (std::min)(delta, pnorm / Scalar(.1));
+ par /= temp;
+ } else if (!(par != 0. && ratio < Scalar(.75))) {
+ delta = pnorm / Scalar(.5);
+ par = Scalar(.5) * par;
+ }
+
+ /* test for successful iteration. */
+ if (ratio >= Scalar(1e-4)) {
+ /* successful iteration. update x, fvec, and their norms. */
+ x = wa2;
+ wa2 = diag.cwiseProduct(x);
+ fvec = wa4;
+ xnorm = wa2.stableNorm();
+ fnorm = fnorm1;
+ ++iter;
+ }
+
+ /* tests for convergence. */
+ if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
+ return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
+ if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
+ return LevenbergMarquardtSpace::RelativeReductionTooSmall;
+ if (delta <= parameters.xtol * xnorm)
+ return LevenbergMarquardtSpace::RelativeErrorTooSmall;
+
+ /* tests for termination and stringent tolerances. */
+ if (nfev >= parameters.maxfev)
+ return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
+ if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
+ return LevenbergMarquardtSpace::FtolTooSmall;
+ if (delta <= NumTraits<Scalar>::epsilon() * xnorm)
+ return LevenbergMarquardtSpace::XtolTooSmall;
+ if (gnorm <= NumTraits<Scalar>::epsilon())
+ return LevenbergMarquardtSpace::GtolTooSmall;
+
+ } while (ratio < Scalar(1e-4));
+
+ return LevenbergMarquardtSpace::Running;
+}
+
+template<typename FunctorType, typename Scalar>
+LevenbergMarquardtSpace::Status
+LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorage(FVectorType &x)
+{
+ LevenbergMarquardtSpace::Status status = minimizeOptimumStorageInit(x);
+ if (status==LevenbergMarquardtSpace::ImproperInputParameters)
+ return status;
+ do {
+ status = minimizeOptimumStorageOneStep(x);
+ } while (status==LevenbergMarquardtSpace::Running);
+ return status;
+}
+
+template<typename FunctorType, typename Scalar>
+LevenbergMarquardtSpace::Status
+LevenbergMarquardt<FunctorType,Scalar>::lmdif1(
+ FunctorType &functor,
+ FVectorType &x,
+ Index *nfev,
+ const Scalar tol
+ )
+{
+ Index n = x.size();
+ Index m = functor.values();
+
+ /* check the input parameters for errors. */
+ if (n <= 0 || m < n || tol < 0.)
+ return LevenbergMarquardtSpace::ImproperInputParameters;
+
+ NumericalDiff<FunctorType> numDiff(functor);
+ // embedded LevenbergMarquardt
+ LevenbergMarquardt<NumericalDiff<FunctorType>, Scalar > lm(numDiff);
+ lm.parameters.ftol = tol;
+ lm.parameters.xtol = tol;
+ lm.parameters.maxfev = 200*(n+1);
+
+ LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x));
+ if (nfev)
+ * nfev = lm.nfev;
+ return info;
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_LEVENBERGMARQUARDT__H
+
+//vim: ai ts=4 sts=4 et sw=4
diff --git a/unsupported/Eigen/src/NonLinearOptimization/chkder.h b/unsupported/Eigen/src/NonLinearOptimization/chkder.h
new file mode 100644
index 0000000..db8ff7d
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/chkder.h
@@ -0,0 +1,66 @@
+#define chkder_log10e 0.43429448190325182765
+#define chkder_factor 100.
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename Scalar>
+void chkder(
+ const Matrix< Scalar, Dynamic, 1 > &x,
+ const Matrix< Scalar, Dynamic, 1 > &fvec,
+ const Matrix< Scalar, Dynamic, Dynamic > &fjac,
+ Matrix< Scalar, Dynamic, 1 > &xp,
+ const Matrix< Scalar, Dynamic, 1 > &fvecp,
+ int mode,
+ Matrix< Scalar, Dynamic, 1 > &err
+ )
+{
+ using std::sqrt;
+ using std::abs;
+ using std::log;
+
+ typedef DenseIndex Index;
+
+ const Scalar eps = sqrt(NumTraits<Scalar>::epsilon());
+ const Scalar epsf = chkder_factor * NumTraits<Scalar>::epsilon();
+ const Scalar epslog = chkder_log10e * log(eps);
+ Scalar temp;
+
+ const Index m = fvec.size(), n = x.size();
+
+ if (mode != 2) {
+ /* mode = 1. */
+ xp.resize(n);
+ for (Index j = 0; j < n; ++j) {
+ temp = eps * abs(x[j]);
+ if (temp == 0.)
+ temp = eps;
+ xp[j] = x[j] + temp;
+ }
+ }
+ else {
+ /* mode = 2. */
+ err.setZero(m);
+ for (Index j = 0; j < n; ++j) {
+ temp = abs(x[j]);
+ if (temp == 0.)
+ temp = 1.;
+ err += temp * fjac.col(j);
+ }
+ for (Index i = 0; i < m; ++i) {
+ temp = 1.;
+ if (fvec[i] != 0. && fvecp[i] != 0. && abs(fvecp[i] - fvec[i]) >= epsf * abs(fvec[i]))
+ temp = eps * abs((fvecp[i] - fvec[i]) / eps - err[i]) / (abs(fvec[i]) + abs(fvecp[i]));
+ err[i] = 1.;
+ if (temp > NumTraits<Scalar>::epsilon() && temp < eps)
+ err[i] = (chkder_log10e * log(temp) - epslog) / epslog;
+ if (temp >= eps)
+ err[i] = 0.;
+ }
+ }
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
diff --git a/unsupported/Eigen/src/NonLinearOptimization/covar.h b/unsupported/Eigen/src/NonLinearOptimization/covar.h
new file mode 100644
index 0000000..68260d1
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/covar.h
@@ -0,0 +1,70 @@
+namespace Eigen {
+
+namespace internal {
+
+template <typename Scalar>
+void covar(
+ Matrix< Scalar, Dynamic, Dynamic > &r,
+ const VectorXi &ipvt,
+ Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) )
+{
+ using std::abs;
+ typedef DenseIndex Index;
+
+ /* Local variables */
+ Index i, j, k, l, ii, jj;
+ bool sing;
+ Scalar temp;
+
+ /* Function Body */
+ const Index n = r.cols();
+ const Scalar tolr = tol * abs(r(0,0));
+ Matrix< Scalar, Dynamic, 1 > wa(n);
+ eigen_assert(ipvt.size()==n);
+
+ /* form the inverse of r in the full upper triangle of r. */
+ l = -1;
+ for (k = 0; k < n; ++k)
+ if (abs(r(k,k)) > tolr) {
+ r(k,k) = 1. / r(k,k);
+ for (j = 0; j <= k-1; ++j) {
+ temp = r(k,k) * r(j,k);
+ r(j,k) = 0.;
+ 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. */
+ for (k = 0; k <= l; ++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. */
+ for (j = 0; j < n; ++j) {
+ jj = ipvt[j];
+ sing = j > l;
+ for (i = 0; i <= j; ++i) {
+ if (sing)
+ r(i,j) = 0.;
+ ii = ipvt[i];
+ if (ii > jj)
+ r(ii,jj) = r(i,j);
+ if (ii < jj)
+ r(jj,ii) = r(i,j);
+ }
+ wa[jj] = r(j,j);
+ }
+
+ /* symmetrize the covariance matrix in r. */
+ r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
+ r.diagonal() = wa;
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
diff --git a/unsupported/Eigen/src/NonLinearOptimization/dogleg.h b/unsupported/Eigen/src/NonLinearOptimization/dogleg.h
new file mode 100644
index 0000000..80c5d27
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/dogleg.h
@@ -0,0 +1,107 @@
+namespace Eigen {
+
+namespace internal {
+
+template <typename Scalar>
+void dogleg(
+ const Matrix< Scalar, Dynamic, Dynamic > &qrfac,
+ const Matrix< Scalar, Dynamic, 1 > &diag,
+ const Matrix< Scalar, Dynamic, 1 > &qtb,
+ Scalar delta,
+ Matrix< Scalar, Dynamic, 1 > &x)
+{
+ using std::abs;
+ using std::sqrt;
+
+ typedef DenseIndex Index;
+
+ /* Local variables */
+ Index i, j;
+ Scalar sum, temp, alpha, bnorm;
+ Scalar gnorm, qnorm;
+ Scalar sgnorm;
+
+ /* Function Body */
+ const Scalar epsmch = NumTraits<Scalar>::epsilon();
+ const Index n = qrfac.cols();
+ eigen_assert(n==qtb.size());
+ eigen_assert(n==x.size());
+ eigen_assert(n==diag.size());
+ Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n);
+
+ /* first, calculate the gauss-newton direction. */
+ for (j = n-1; j >=0; --j) {
+ temp = qrfac(j,j);
+ if (temp == 0.) {
+ temp = epsmch * qrfac.col(j).head(j+1).maxCoeff();
+ if (temp == 0.)
+ temp = epsmch;
+ }
+ if (j==n-1)
+ x[j] = qtb[j] / temp;
+ else
+ x[j] = (qtb[j] - qrfac.row(j).tail(n-j-1).dot(x.tail(n-j-1))) / temp;
+ }
+
+ /* test whether the gauss-newton direction is acceptable. */
+ qnorm = diag.cwiseProduct(x).stableNorm();
+ if (qnorm <= delta)
+ return;
+
+ // TODO : this path is not tested by Eigen unit tests
+
+ /* the gauss-newton direction is not acceptable. */
+ /* next, calculate the scaled gradient direction. */
+
+ wa1.fill(0.);
+ for (j = 0; j < n; ++j) {
+ wa1.tail(n-j) += qrfac.row(j).tail(n-j) * qtb[j];
+ wa1[j] /= diag[j];
+ }
+
+ /* calculate the norm of the scaled gradient and test for */
+ /* the special case in which the scaled gradient is zero. */
+ gnorm = wa1.stableNorm();
+ sgnorm = 0.;
+ alpha = delta / qnorm;
+ if (gnorm == 0.)
+ goto algo_end;
+
+ /* calculate the point along the scaled gradient */
+ /* at which the quadratic is minimized. */
+ wa1.array() /= (diag*gnorm).array();
+ // TODO : once unit tests cover this part,:
+ // wa2 = qrfac.template triangularView<Upper>() * wa1;
+ for (j = 0; j < n; ++j) {
+ sum = 0.;
+ for (i = j; i < n; ++i) {
+ sum += qrfac(j,i) * wa1[i];
+ }
+ wa2[j] = sum;
+ }
+ temp = wa2.stableNorm();
+ sgnorm = gnorm / temp / temp;
+
+ /* test whether the scaled gradient direction is acceptable. */
+ alpha = 0.;
+ if (sgnorm >= delta)
+ goto algo_end;
+
+ /* the scaled gradient direction is not acceptable. */
+ /* finally, calculate the point along the dogleg */
+ /* at which the quadratic is minimized. */
+ bnorm = qtb.stableNorm();
+ temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
+ temp = temp - delta / qnorm * numext::abs2(sgnorm / delta) + sqrt(numext::abs2(temp - delta / qnorm) + (1.-numext::abs2(delta / qnorm)) * (1.-numext::abs2(sgnorm / delta)));
+ alpha = delta / qnorm * (1. - numext::abs2(sgnorm / delta)) / temp;
+algo_end:
+
+ /* form appropriate convex combination of the gauss-newton */
+ /* direction and the scaled gradient direction. */
+ temp = (1.-alpha) * (std::min)(sgnorm,delta);
+ x = temp * wa1 + alpha * x;
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
diff --git a/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h b/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h
new file mode 100644
index 0000000..bb7cf26
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h
@@ -0,0 +1,79 @@
+namespace Eigen {
+
+namespace internal {
+
+template<typename FunctorType, typename Scalar>
+DenseIndex fdjac1(
+ const FunctorType &Functor,
+ Matrix< Scalar, Dynamic, 1 > &x,
+ Matrix< Scalar, Dynamic, 1 > &fvec,
+ Matrix< Scalar, Dynamic, Dynamic > &fjac,
+ DenseIndex ml, DenseIndex mu,
+ Scalar epsfcn)
+{
+ using std::sqrt;
+ using std::abs;
+
+ typedef DenseIndex Index;
+
+ /* Local variables */
+ Scalar h;
+ Index j, k;
+ Scalar eps, temp;
+ Index msum;
+ int iflag;
+ Index start, length;
+
+ /* Function Body */
+ const Scalar epsmch = NumTraits<Scalar>::epsilon();
+ const Index n = x.size();
+ eigen_assert(fvec.size()==n);
+ Matrix< Scalar, Dynamic, 1 > wa1(n);
+ Matrix< Scalar, Dynamic, 1 > wa2(n);
+
+ eps = sqrt((std::max)(epsfcn,epsmch));
+ msum = ml + mu + 1;
+ if (msum >= n) {
+ /* computation of dense approximate jacobian. */
+ for (j = 0; j < n; ++j) {
+ temp = x[j];
+ h = eps * abs(temp);
+ if (h == 0.)
+ h = eps;
+ x[j] = temp + h;
+ iflag = Functor(x, wa1);
+ if (iflag < 0)
+ return iflag;
+ x[j] = temp;
+ fjac.col(j) = (wa1-fvec)/h;
+ }
+
+ }else {
+ /* computation of banded approximate jacobian. */
+ for (k = 0; k < msum; ++k) {
+ for (j = k; (msum<0) ? (j>n): (j<n); j += msum) {
+ wa2[j] = x[j];
+ h = eps * abs(wa2[j]);
+ if (h == 0.) h = eps;
+ x[j] = wa2[j] + h;
+ }
+ iflag = Functor(x, wa1);
+ if (iflag < 0)
+ return iflag;
+ for (j = k; (msum<0) ? (j>n): (j<n); j += msum) {
+ x[j] = wa2[j];
+ h = eps * abs(wa2[j]);
+ if (h == 0.) h = eps;
+ fjac.col(j).setZero();
+ start = std::max<Index>(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;
+ }
+ }
+ }
+ return 0;
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
diff --git a/unsupported/Eigen/src/NonLinearOptimization/lmpar.h b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h
new file mode 100644
index 0000000..4c17d4c
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h
@@ -0,0 +1,298 @@
+namespace Eigen {
+
+namespace internal {
+
+template <typename Scalar>
+void lmpar(
+ Matrix< Scalar, Dynamic, Dynamic > &r,
+ const VectorXi &ipvt,
+ const Matrix< Scalar, Dynamic, 1 > &diag,
+ const Matrix< Scalar, Dynamic, 1 > &qtb,
+ Scalar delta,
+ Scalar &par,
+ Matrix< Scalar, Dynamic, 1 > &x)
+{
+ using std::abs;
+ using std::sqrt;
+ typedef DenseIndex Index;
+
+ /* Local variables */
+ Index i, j, l;
+ 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 = r.cols();
+ eigen_assert(n==diag.size());
+ eigen_assert(n==qtb.size());
+ eigen_assert(n==x.size());
+
+ Matrix< Scalar, Dynamic, 1 > wa1, wa2;
+
+ /* compute and store in x the gauss-newton direction. if the */
+ /* jacobian is rank-deficient, obtain a least squares solution. */
+ Index nsing = n-1;
+ wa1 = qtb;
+ for (j = 0; j < n; ++j) {
+ if (r(j,j) == 0. && nsing == n-1)
+ nsing = j - 1;
+ if (nsing < n-1)
+ wa1[j] = 0.;
+ }
+ for (j = nsing; j>=0; --j) {
+ wa1[j] /= r(j,j);
+ temp = wa1[j];
+ for (i = 0; i < j ; ++i)
+ wa1[i] -= r(i,j) * temp;
+ }
+
+ for (j = 0; j < n; ++j)
+ x[ipvt[j]] = wa1[j];
+
+ /* 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 - delta;
+ if (fp <= Scalar(0.1) * 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 (nsing >= n-1) {
+ for (j = 0; j < n; ++j) {
+ l = ipvt[j];
+ wa1[j] = diag[l] * (wa2[l] / dxnorm);
+ }
+ // it's actually a triangularView.solveInplace(), though in a weird
+ // way:
+ for (j = 0; j < n; ++j) {
+ Scalar sum = 0.;
+ for (i = 0; i < j; ++i)
+ sum += r(i,j) * wa1[i];
+ wa1[j] = (wa1[j] - sum) / r(j,j);
+ }
+ temp = wa1.blueNorm();
+ parl = fp / delta / temp / temp;
+ }
+
+ /* calculate an upper bound, paru, for the zero of the function. */
+ for (j = 0; j < n; ++j)
+ wa1[j] = r.col(j).head(j+1).dot(qtb.head(j+1)) / diag[ipvt[j]];
+
+ gnorm = wa1.stableNorm();
+ paru = gnorm / delta;
+ if (paru == 0.)
+ paru = dwarf / (std::min)(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. */
+ 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;
+
+ Matrix< Scalar, Dynamic, 1 > sdiag(n);
+ qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag);
+
+ wa2 = diag.cwiseProduct(x);
+ dxnorm = wa2.blueNorm();
+ temp = fp;
+ fp = dxnorm - 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) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
+ break;
+
+ /* compute the newton correction. */
+ for (j = 0; j < n; ++j) {
+ l = ipvt[j];
+ wa1[j] = diag[l] * (wa2[l] / dxnorm);
+ }
+ for (j = 0; j < n; ++j) {
+ wa1[j] /= sdiag[j];
+ temp = wa1[j];
+ for (i = j+1; i < n; ++i)
+ wa1[i] -= r(i,j) * temp;
+ }
+ temp = wa1.blueNorm();
+ parc = fp / 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. */
+ /* Computing MAX */
+ par = (std::max)(parl,par+parc);
+
+ /* end of an iteration. */
+ }
+
+ /* termination. */
+ if (iter == 0)
+ par = 0.;
+ return;
+}
+
+template <typename Scalar>
+void lmpar2(
+ const ColPivHouseholderQR<Matrix< Scalar, Dynamic, Dynamic> > &qr,
+ const Matrix< Scalar, Dynamic, 1 > &diag,
+ const Matrix< Scalar, Dynamic, 1 > &qtb,
+ Scalar delta,
+ Scalar &par,
+ Matrix< Scalar, Dynamic, 1 > &x)
+
+{
+ using std::sqrt;
+ using std::abs;
+ typedef DenseIndex 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();
+ eigen_assert(n==diag.size());
+ eigen_assert(n==qtb.size());
+
+ Matrix< Scalar, Dynamic, 1 > 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();
+ qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.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 - delta;
+ if (fp <= Scalar(0.1) * 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 / 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 / delta;
+ if (paru == 0.)
+ paru = dwarf / (std::min)(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. */
+ Matrix< Scalar, Dynamic, Dynamic > 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;
+
+ Matrix< Scalar, Dynamic, 1 > sdiag(n);
+ qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag);
+
+ wa2 = diag.cwiseProduct(x);
+ dxnorm = wa2.blueNorm();
+ temp = fp;
+ fp = dxnorm - 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) * 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(i,j) * temp;
+ }
+ temp = wa1.blueNorm();
+ parc = fp / 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
diff --git a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
new file mode 100644
index 0000000..feafd62
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
@@ -0,0 +1,91 @@
+namespace Eigen {
+
+namespace internal {
+
+// TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt
+template <typename Scalar>
+void qrsolv(
+ Matrix< Scalar, Dynamic, Dynamic > &s,
+ // TODO : use a PermutationMatrix once lmpar is no more:
+ const VectorXi &ipvt,
+ const Matrix< Scalar, Dynamic, 1 > &diag,
+ const Matrix< Scalar, Dynamic, 1 > &qtb,
+ Matrix< Scalar, Dynamic, 1 > &x,
+ Matrix< Scalar, Dynamic, 1 > &sdiag)
+
+{
+ typedef DenseIndex Index;
+
+ /* 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 = ipvt[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. */
+ for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j];
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
diff --git a/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h b/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h
new file mode 100644
index 0000000..36ff700
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h
@@ -0,0 +1,30 @@
+namespace Eigen {
+
+namespace internal {
+
+// TODO : move this to GivensQR once there's such a thing in Eigen
+
+template <typename Scalar>
+void r1mpyq(DenseIndex m, DenseIndex n, Scalar *a, const std::vector<JacobiRotation<Scalar> > &v_givens, const std::vector<JacobiRotation<Scalar> > &w_givens)
+{
+ typedef DenseIndex Index;
+
+ /* apply the first set of givens rotations to a. */
+ for (Index j = n-2; j>=0; --j)
+ for (Index i = 0; i<m; ++i) {
+ Scalar temp = v_givens[j].c() * a[i+m*j] - v_givens[j].s() * a[i+m*(n-1)];
+ a[i+m*(n-1)] = v_givens[j].s() * a[i+m*j] + v_givens[j].c() * a[i+m*(n-1)];
+ a[i+m*j] = temp;
+ }
+ /* apply the second set of givens rotations to a. */
+ for (Index j = 0; j<n-1; ++j)
+ for (Index i = 0; i<m; ++i) {
+ Scalar temp = w_givens[j].c() * a[i+m*j] + w_givens[j].s() * a[i+m*(n-1)];
+ a[i+m*(n-1)] = -w_givens[j].s() * a[i+m*j] + w_givens[j].c() * a[i+m*(n-1)];
+ a[i+m*j] = temp;
+ }
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
diff --git a/unsupported/Eigen/src/NonLinearOptimization/r1updt.h b/unsupported/Eigen/src/NonLinearOptimization/r1updt.h
new file mode 100644
index 0000000..f287660
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/r1updt.h
@@ -0,0 +1,99 @@
+namespace Eigen {
+
+namespace internal {
+
+template <typename Scalar>
+void r1updt(
+ Matrix< Scalar, Dynamic, Dynamic > &s,
+ const Matrix< Scalar, Dynamic, 1> &u,
+ std::vector<JacobiRotation<Scalar> > &v_givens,
+ std::vector<JacobiRotation<Scalar> > &w_givens,
+ Matrix< Scalar, Dynamic, 1> &v,
+ Matrix< Scalar, Dynamic, 1> &w,
+ bool *sing)
+{
+ typedef DenseIndex Index;
+ const JacobiRotation<Scalar> IdentityRotation = JacobiRotation<Scalar>(1,0);
+
+ /* Local variables */
+ const Index m = s.rows();
+ const Index n = s.cols();
+ Index i, j=1;
+ Scalar temp;
+ JacobiRotation<Scalar> givens;
+
+ // r1updt had a broader usecase, but we dont use it here. And, more
+ // importantly, we can not test it.
+ eigen_assert(m==n);
+ eigen_assert(u.size()==m);
+ eigen_assert(v.size()==n);
+ eigen_assert(w.size()==n);
+
+ /* move the nontrivial part of the last column of s into w. */
+ w[n-1] = s(n-1,n-1);
+
+ /* rotate the vector v into a multiple of the n-th unit vector */
+ /* in such a way that a spike is introduced into w. */
+ for (j=n-2; j>=0; --j) {
+ w[j] = 0.;
+ if (v[j] != 0.) {
+ /* determine a givens rotation which eliminates the */
+ /* j-th element of v. */
+ givens.makeGivens(-v[n-1], v[j]);
+
+ /* apply the transformation to v and store the information */
+ /* necessary to recover the givens rotation. */
+ v[n-1] = givens.s() * v[j] + givens.c() * v[n-1];
+ v_givens[j] = givens;
+
+ /* apply the transformation to s and extend the spike in w. */
+ for (i = j; i < m; ++i) {
+ temp = givens.c() * s(j,i) - givens.s() * w[i];
+ w[i] = givens.s() * s(j,i) + givens.c() * w[i];
+ s(j,i) = temp;
+ }
+ } else
+ v_givens[j] = IdentityRotation;
+ }
+
+ /* add the spike from the rank 1 update to w. */
+ w += v[n-1] * u;
+
+ /* eliminate the spike. */
+ *sing = false;
+ for (j = 0; j < n-1; ++j) {
+ if (w[j] != 0.) {
+ /* determine a givens rotation which eliminates the */
+ /* j-th element of the spike. */
+ givens.makeGivens(-s(j,j), w[j]);
+
+ /* apply the transformation to s and reduce the spike in w. */
+ for (i = j; i < m; ++i) {
+ temp = givens.c() * s(j,i) + givens.s() * w[i];
+ w[i] = -givens.s() * s(j,i) + givens.c() * w[i];
+ s(j,i) = temp;
+ }
+
+ /* store the information necessary to recover the */
+ /* givens rotation. */
+ w_givens[j] = givens;
+ } else
+ v_givens[j] = IdentityRotation;
+
+ /* test for zero diagonal elements in the output s. */
+ if (s(j,j) == 0.) {
+ *sing = true;
+ }
+ }
+ /* move w back into the last column of the output s. */
+ s(n-1,n-1) = w[n-1];
+
+ if (s(j,j) == 0.) {
+ *sing = true;
+ }
+ return;
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
diff --git a/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h b/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h
new file mode 100644
index 0000000..6ebf856
--- /dev/null
+++ b/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h
@@ -0,0 +1,49 @@
+namespace Eigen {
+
+namespace internal {
+
+template <typename Scalar>
+void rwupdt(
+ Matrix< Scalar, Dynamic, Dynamic > &r,
+ const Matrix< Scalar, Dynamic, 1> &w,
+ Matrix< Scalar, Dynamic, 1> &b,
+ Scalar alpha)
+{
+ typedef DenseIndex Index;
+
+ const Index n = r.cols();
+ eigen_assert(r.rows()>=n);
+ std::vector<JacobiRotation<Scalar> > givens(n);
+
+ /* Local variables */
+ Scalar temp, rowj;
+
+ /* Function Body */
+ for (Index j = 0; j < n; ++j) {
+ rowj = w[j];
+
+ /* apply the previous transformations to */
+ /* r(i,j), i=0,1,...,j-1, and to w(j). */
+ for (Index 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;
+ }
+
+ /* determine a givens rotation which eliminates w(j). */
+ givens[j].makeGivens(-r(j,j), rowj);
+
+ if (rowj == 0.)
+ continue; // givens[j] is identity
+
+ /* 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;
+ }
+}
+
+} // end namespace internal
+
+} // end namespace Eigen