add vcglib

vcglib/eigenlib/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 the mozilla.org home page
+
+#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(numext::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 = numext::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 = numext::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

vcglib/eigenlib/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h

@@ -0,0 +1,657 @@
+// -*- 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 the mozilla.org home page
+
+#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.
+  * xxxp://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
+  */
+template<typename FunctorType, typename Scalar=double>
+class LevenbergMarquardt
+{
+    static Scalar sqrt_epsilon()
+    {
+      using std::sqrt;
+      return sqrt(NumTraits<Scalar>::epsilon());
+    }
+    
+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(sqrt_epsilon())
+            , xtol(sqrt_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 = sqrt_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 = sqrt_epsilon()
+            );
+
+    LevenbergMarquardtSpace::Status lmstr1(
+            FVectorType  &x,
+            const Scalar tol = sqrt_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

vcglib/eigenlib/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

vcglib/eigenlib/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

vcglib/eigenlib/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

vcglib/eigenlib/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

vcglib/eigenlib/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

vcglib/eigenlib/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 transformation 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

vcglib/eigenlib/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

vcglib/eigenlib/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 don't 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

vcglib/eigenlib/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