diff options
Diffstat (limited to 'eigen/unsupported/Eigen/src/LevenbergMarquardt')
6 files changed, 0 insertions, 1082 deletions
diff --git a/eigen/unsupported/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt b/eigen/unsupported/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt deleted file mode 100644 index ae7984d..0000000 --- a/eigen/unsupported/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt +++ /dev/null @@ -1,52 +0,0 @@ -Minpack Copyright Notice (1999) University of Chicago. All rights reserved - -Redistribution and use in source and binary forms, with or -without modification, are permitted provided that the -following conditions are met: - -1. Redistributions of source code must retain the above -copyright notice, this list of conditions and the following -disclaimer. - -2. Redistributions in binary form must reproduce the above -copyright notice, this list of conditions and the following -disclaimer in the documentation and/or other materials -provided with the distribution. - -3. The end-user documentation included with the -redistribution, if any, must include the following -acknowledgment: - - "This product includes software developed by the - University of Chicago, as Operator of Argonne National - Laboratory. - -Alternately, this acknowledgment may appear in the software -itself, if and wherever such third-party acknowledgments -normally appear. - -4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS" -WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE -UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND -THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES -OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE -OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY -OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR -USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF -THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4) -DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION -UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL -BE CORRECTED. - -5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT -HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF -ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT, -INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF -ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF -PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER -SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT -(INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE, -EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE -POSSIBILITY OF SUCH LOSS OR DAMAGES. - diff --git a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMcovar.h b/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMcovar.h deleted file mode 100644 index b75bea2..0000000 --- a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMcovar.h +++ /dev/null @@ -1,84 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// This code initially comes from MINPACK whose original authors are: -// Copyright Jorge More - Argonne National Laboratory -// Copyright Burt Garbow - Argonne National Laboratory -// Copyright Ken Hillstrom - Argonne National Laboratory -// -// This Source Code Form is subject to the terms of the Minpack license -// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. - -#ifndef EIGEN_LMCOVAR_H -#define EIGEN_LMCOVAR_H - -namespace Eigen { - -namespace internal { - -template <typename Scalar> -void covar( - Matrix< Scalar, Dynamic, Dynamic > &r, - const VectorXi& ipvt, - Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) ) -{ - using std::abs; - /* 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 - -#endif // EIGEN_LMCOVAR_H diff --git a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h b/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h deleted file mode 100644 index 25b32ec..0000000 --- a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h +++ /dev/null @@ -1,202 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> -// -// This code initially comes from MINPACK whose original authors are: -// Copyright Jorge More - Argonne National Laboratory -// Copyright Burt Garbow - Argonne National Laboratory -// Copyright Ken Hillstrom - Argonne National Laboratory -// -// This Source Code Form is subject to the terms of the Minpack license -// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. - -#ifndef EIGEN_LMONESTEP_H -#define EIGEN_LMONESTEP_H - -namespace Eigen { - -template<typename FunctorType> -LevenbergMarquardtSpace::Status -LevenbergMarquardt<FunctorType>::minimizeOneStep(FVectorType &x) -{ - using std::abs; - using std::sqrt; - RealScalar temp, temp1,temp2; - RealScalar ratio; - RealScalar pnorm, xnorm, fnorm1, actred, dirder, prered; - eigen_assert(x.size()==n); // check the caller is not cheating us - - temp = 0.0; xnorm = 0.0; - /* calculate the jacobian matrix. */ - Index df_ret = m_functor.df(x, m_fjac); - if (df_ret<0) - return LevenbergMarquardtSpace::UserAsked; - if (df_ret>0) - // numerical diff, we evaluated the function df_ret times - m_nfev += df_ret; - else m_njev++; - - /* compute the qr factorization of the jacobian. */ - for (int j = 0; j < x.size(); ++j) - m_wa2(j) = m_fjac.col(j).blueNorm(); - QRSolver qrfac(m_fjac); - if(qrfac.info() != Success) { - m_info = NumericalIssue; - return LevenbergMarquardtSpace::ImproperInputParameters; - } - // Make a copy of the first factor with the associated permutation - m_rfactor = qrfac.matrixR(); - m_permutation = (qrfac.colsPermutation()); - - /* on the first iteration and if external scaling is not used, scale according */ - /* to the norms of the columns of the initial jacobian. */ - if (m_iter == 1) { - if (!m_useExternalScaling) - for (Index j = 0; j < n; ++j) - m_diag[j] = (m_wa2[j]==0.)? 1. : m_wa2[j]; - - /* on the first iteration, calculate the norm of the scaled x */ - /* and initialize the step bound m_delta. */ - xnorm = m_diag.cwiseProduct(x).stableNorm(); - m_delta = m_factor * xnorm; - if (m_delta == 0.) - m_delta = m_factor; - } - - /* form (q transpose)*m_fvec and store the first n components in */ - /* m_qtf. */ - m_wa4 = m_fvec; - m_wa4 = qrfac.matrixQ().adjoint() * m_fvec; - m_qtf = m_wa4.head(n); - - /* compute the norm of the scaled gradient. */ - m_gnorm = 0.; - if (m_fnorm != 0.) - for (Index j = 0; j < n; ++j) - if (m_wa2[m_permutation.indices()[j]] != 0.) - m_gnorm = (std::max)(m_gnorm, abs( m_rfactor.col(j).head(j+1).dot(m_qtf.head(j+1)/m_fnorm) / m_wa2[m_permutation.indices()[j]])); - - /* test for convergence of the gradient norm. */ - if (m_gnorm <= m_gtol) { - m_info = Success; - return LevenbergMarquardtSpace::CosinusTooSmall; - } - - /* rescale if necessary. */ - if (!m_useExternalScaling) - m_diag = m_diag.cwiseMax(m_wa2); - - do { - /* determine the levenberg-marquardt parameter. */ - internal::lmpar2(qrfac, m_diag, m_qtf, m_delta, m_par, m_wa1); - - /* store the direction p and x + p. calculate the norm of p. */ - m_wa1 = -m_wa1; - m_wa2 = x + m_wa1; - pnorm = m_diag.cwiseProduct(m_wa1).stableNorm(); - - /* on the first iteration, adjust the initial step bound. */ - if (m_iter == 1) - m_delta = (std::min)(m_delta,pnorm); - - /* evaluate the function at x + p and calculate its norm. */ - if ( m_functor(m_wa2, m_wa4) < 0) - return LevenbergMarquardtSpace::UserAsked; - ++m_nfev; - fnorm1 = m_wa4.stableNorm(); - - /* compute the scaled actual reduction. */ - actred = -1.; - if (Scalar(.1) * fnorm1 < m_fnorm) - actred = 1. - numext::abs2(fnorm1 / m_fnorm); - - /* compute the scaled predicted reduction and */ - /* the scaled directional derivative. */ - m_wa3 = m_rfactor.template triangularView<Upper>() * (m_permutation.inverse() *m_wa1); - temp1 = numext::abs2(m_wa3.stableNorm() / m_fnorm); - temp2 = numext::abs2(sqrt(m_par) * pnorm / m_fnorm); - prered = temp1 + temp2 / Scalar(.5); - dirder = -(temp1 + temp2); - - /* compute the ratio of the actual to the predicted */ - /* reduction. */ - ratio = 0.; - if (prered != 0.) - ratio = actred / prered; - - /* update the step bound. */ - if (ratio <= Scalar(.25)) { - if (actred >= 0.) - temp = RealScalar(.5); - if (actred < 0.) - temp = RealScalar(.5) * dirder / (dirder + RealScalar(.5) * actred); - if (RealScalar(.1) * fnorm1 >= m_fnorm || temp < RealScalar(.1)) - temp = Scalar(.1); - /* Computing MIN */ - m_delta = temp * (std::min)(m_delta, pnorm / RealScalar(.1)); - m_par /= temp; - } else if (!(m_par != 0. && ratio < RealScalar(.75))) { - m_delta = pnorm / RealScalar(.5); - m_par = RealScalar(.5) * m_par; - } - - /* test for successful iteration. */ - if (ratio >= RealScalar(1e-4)) { - /* successful iteration. update x, m_fvec, and their norms. */ - x = m_wa2; - m_wa2 = m_diag.cwiseProduct(x); - m_fvec = m_wa4; - xnorm = m_wa2.stableNorm(); - m_fnorm = fnorm1; - ++m_iter; - } - - /* tests for convergence. */ - if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1. && m_delta <= m_xtol * xnorm) - { - m_info = Success; - return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall; - } - if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1.) - { - m_info = Success; - return LevenbergMarquardtSpace::RelativeReductionTooSmall; - } - if (m_delta <= m_xtol * xnorm) - { - m_info = Success; - return LevenbergMarquardtSpace::RelativeErrorTooSmall; - } - - /* tests for termination and stringent tolerances. */ - if (m_nfev >= m_maxfev) - { - m_info = NoConvergence; - return LevenbergMarquardtSpace::TooManyFunctionEvaluation; - } - if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.) - { - m_info = Success; - return LevenbergMarquardtSpace::FtolTooSmall; - } - if (m_delta <= NumTraits<Scalar>::epsilon() * xnorm) - { - m_info = Success; - return LevenbergMarquardtSpace::XtolTooSmall; - } - if (m_gnorm <= NumTraits<Scalar>::epsilon()) - { - m_info = Success; - return LevenbergMarquardtSpace::GtolTooSmall; - } - - } while (ratio < Scalar(1e-4)); - - return LevenbergMarquardtSpace::Running; -} - - -} // end namespace Eigen - -#endif // EIGEN_LMONESTEP_H diff --git a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMpar.h b/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMpar.h deleted file mode 100644 index 9a48365..0000000 --- a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMpar.h +++ /dev/null @@ -1,160 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// This code initially comes from MINPACK whose original authors are: -// Copyright Jorge More - Argonne National Laboratory -// Copyright Burt Garbow - Argonne National Laboratory -// Copyright Ken Hillstrom - Argonne National Laboratory -// -// This Source Code Form is subject to the terms of the Minpack license -// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. - -#ifndef EIGEN_LMPAR_H -#define EIGEN_LMPAR_H - -namespace Eigen { - -namespace internal { - - template <typename QRSolver, typename VectorType> - void lmpar2( - const QRSolver &qr, - const VectorType &diag, - const VectorType &qtb, - typename VectorType::Scalar m_delta, - typename VectorType::Scalar &par, - VectorType &x) - - { - using std::sqrt; - using std::abs; - typedef typename QRSolver::MatrixType MatrixType; - typedef typename QRSolver::Scalar Scalar; -// typedef typename QRSolver::StorageIndex StorageIndex; - - /* Local variables */ - Index j; - Scalar fp; - Scalar parc, parl; - Index iter; - Scalar temp, paru; - Scalar gnorm; - Scalar dxnorm; - - // Make a copy of the triangular factor. - // This copy is modified during call the qrsolv - MatrixType s; - s = qr.matrixR(); - - /* Function Body */ - const Scalar dwarf = (std::numeric_limits<Scalar>::min)(); - const Index n = qr.matrixR().cols(); - eigen_assert(n==diag.size()); - eigen_assert(n==qtb.size()); - - VectorType wa1, wa2; - - /* compute and store in x the gauss-newton direction. if the */ - /* jacobian is rank-deficient, obtain a least squares solution. */ - - // const Index rank = qr.nonzeroPivots(); // exactly double(0.) - const Index rank = qr.rank(); // use a threshold - wa1 = qtb; - wa1.tail(n-rank).setZero(); - //FIXME There is no solve in place for sparse triangularView - wa1.head(rank) = s.topLeftCorner(rank,rank).template triangularView<Upper>().solve(qtb.head(rank)); - - x = qr.colsPermutation()*wa1; - - /* initialize the iteration counter. */ - /* evaluate the function at the origin, and test */ - /* for acceptance of the gauss-newton direction. */ - iter = 0; - wa2 = diag.cwiseProduct(x); - dxnorm = wa2.blueNorm(); - fp = dxnorm - m_delta; - if (fp <= Scalar(0.1) * m_delta) { - par = 0; - return; - } - - /* if the jacobian is not rank deficient, the newton */ - /* step provides a lower bound, parl, for the zero of */ - /* the function. otherwise set this bound to zero. */ - parl = 0.; - if (rank==n) { - wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm; - s.topLeftCorner(n,n).transpose().template triangularView<Lower>().solveInPlace(wa1); - temp = wa1.blueNorm(); - parl = fp / m_delta / temp / temp; - } - - /* calculate an upper bound, paru, for the zero of the function. */ - for (j = 0; j < n; ++j) - wa1[j] = s.col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)]; - - gnorm = wa1.stableNorm(); - paru = gnorm / m_delta; - if (paru == 0.) - paru = dwarf / (std::min)(m_delta,Scalar(0.1)); - - /* if the input par lies outside of the interval (parl,paru), */ - /* set par to the closer endpoint. */ - par = (std::max)(par,parl); - par = (std::min)(par,paru); - if (par == 0.) - par = gnorm / dxnorm; - - /* beginning of an iteration. */ - while (true) { - ++iter; - - /* evaluate the function at the current value of par. */ - if (par == 0.) - par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ - wa1 = sqrt(par)* diag; - - VectorType sdiag(n); - lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag); - - wa2 = diag.cwiseProduct(x); - dxnorm = wa2.blueNorm(); - temp = fp; - fp = dxnorm - m_delta; - - /* if the function is small enough, accept the current value */ - /* of par. also test for the exceptional cases where parl */ - /* is zero or the number of iterations has reached 10. */ - if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) - break; - - /* compute the newton correction. */ - wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm); - // we could almost use this here, but the diagonal is outside qr, in sdiag[] - for (j = 0; j < n; ++j) { - wa1[j] /= sdiag[j]; - temp = wa1[j]; - for (Index i = j+1; i < n; ++i) - wa1[i] -= s.coeff(i,j) * temp; - } - temp = wa1.blueNorm(); - parc = fp / m_delta / temp / temp; - - /* depending on the sign of the function, update parl or paru. */ - if (fp > 0.) - parl = (std::max)(parl,par); - if (fp < 0.) - paru = (std::min)(paru,par); - - /* compute an improved estimate for par. */ - par = (std::max)(parl,par+parc); - } - if (iter == 0) - par = 0.; - return; - } -} // end namespace internal - -} // end namespace Eigen - -#endif // EIGEN_LMPAR_H diff --git a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h b/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h deleted file mode 100644 index ae9d793..0000000 --- a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h +++ /dev/null @@ -1,188 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> -// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr> -// -// This code initially comes from MINPACK whose original authors are: -// Copyright Jorge More - Argonne National Laboratory -// Copyright Burt Garbow - Argonne National Laboratory -// Copyright Ken Hillstrom - Argonne National Laboratory -// -// This Source Code Form is subject to the terms of the Minpack license -// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. - -#ifndef EIGEN_LMQRSOLV_H -#define EIGEN_LMQRSOLV_H - -namespace Eigen { - -namespace internal { - -template <typename Scalar,int Rows, int Cols, typename PermIndex> -void lmqrsolv( - Matrix<Scalar,Rows,Cols> &s, - const PermutationMatrix<Dynamic,Dynamic,PermIndex> &iPerm, - const Matrix<Scalar,Dynamic,1> &diag, - const Matrix<Scalar,Dynamic,1> &qtb, - Matrix<Scalar,Dynamic,1> &x, - Matrix<Scalar,Dynamic,1> &sdiag) -{ - /* Local variables */ - Index i, j, k; - 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. */ - const PermIndex l = iPerm.indices()(j); - if (diag[l] == 0.) - break; - sdiag.tail(n-j).setZero(); - sdiag[j] = diag[l]; - - /* the transformations to eliminate the row of d */ - /* modify only a single element of (q transpose)*b */ - /* beyond the first n, which is initially zero. */ - Scalar qtbpj = 0.; - for (k = j; k < n; ++k) { - /* determine a givens rotation which eliminates the */ - /* appropriate element in the current row of d. */ - givens.makeGivens(-s(k,k), sdiag[k]); - - /* compute the modified diagonal element of r and */ - /* the modified element of ((q transpose)*b,0). */ - s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k]; - temp = givens.c() * wa[k] + givens.s() * qtbpj; - qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj; - wa[k] = temp; - - /* accumulate the tranformation in the row of s. */ - for (i = k+1; i<n; ++i) { - temp = givens.c() * s(i,k) + givens.s() * sdiag[i]; - sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i]; - s(i,k) = temp; - } - } - } - - /* solve the triangular system for z. if the system is */ - /* singular, then obtain a least squares solution. */ - Index nsing; - for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {} - - wa.tail(n-nsing).setZero(); - s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing)); - - // restore - sdiag = s.diagonal(); - s.diagonal() = x; - - /* permute the components of z back to components of x. */ - x = iPerm * wa; -} - -template <typename Scalar, int _Options, typename Index> -void lmqrsolv( - SparseMatrix<Scalar,_Options,Index> &s, - const PermutationMatrix<Dynamic,Dynamic> &iPerm, - const Matrix<Scalar,Dynamic,1> &diag, - const Matrix<Scalar,Dynamic,1> &qtb, - Matrix<Scalar,Dynamic,1> &x, - Matrix<Scalar,Dynamic,1> &sdiag) -{ - /* Local variables */ - typedef SparseMatrix<Scalar,RowMajor,Index> FactorType; - Index i, j, k, l; - Scalar temp; - Index n = s.cols(); - Matrix<Scalar,Dynamic,1> wa(n); - JacobiRotation<Scalar> givens; - - /* Function Body */ - // the following will only change the lower triangular part of s, including - // the diagonal, though the diagonal is restored afterward - - /* copy r and (q transpose)*b to preserve input and initialize R. */ - wa = qtb; - FactorType R(s); - // Eliminate the diagonal matrix d using a givens rotation - for (j = 0; j < n; ++j) - { - // Prepare the row of d to be eliminated, locating the - // diagonal element using p from the qr factorization - l = iPerm.indices()(j); - if (diag(l) == Scalar(0)) - break; - sdiag.tail(n-j).setZero(); - sdiag[j] = diag[l]; - // the transformations to eliminate the row of d - // modify only a single element of (q transpose)*b - // beyond the first n, which is initially zero. - - Scalar qtbpj = 0; - // Browse the nonzero elements of row j of the upper triangular s - for (k = j; k < n; ++k) - { - typename FactorType::InnerIterator itk(R,k); - for (; itk; ++itk){ - if (itk.index() < k) continue; - else break; - } - //At this point, we have the diagonal element R(k,k) - // Determine a givens rotation which eliminates - // the appropriate element in the current row of d - givens.makeGivens(-itk.value(), sdiag(k)); - - // Compute the modified diagonal element of r and - // the modified element of ((q transpose)*b,0). - itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k); - temp = givens.c() * wa(k) + givens.s() * qtbpj; - qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj; - wa(k) = temp; - - // Accumulate the transformation in the remaining k row/column of R - for (++itk; itk; ++itk) - { - i = itk.index(); - temp = givens.c() * itk.value() + givens.s() * sdiag(i); - sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i); - itk.valueRef() = temp; - } - } - } - - // Solve the triangular system for z. If the system is - // singular, then obtain a least squares solution - Index nsing; - for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {} - - wa.tail(n-nsing).setZero(); -// x = wa; - wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing)); - - sdiag = R.diagonal(); - // Permute the components of z back to components of x - x = iPerm * wa; -} -} // end namespace internal - -} // end namespace Eigen - -#endif // EIGEN_LMQRSOLV_H diff --git a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h b/eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h deleted file mode 100644 index 9954279..0000000 --- a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h +++ /dev/null @@ -1,396 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> -// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr> -// -// The algorithm of this class initially comes from MINPACK whose original authors are: -// Copyright Jorge More - Argonne National Laboratory -// Copyright Burt Garbow - Argonne National Laboratory -// Copyright Ken Hillstrom - Argonne National Laboratory -// -// This Source Code Form is subject to the terms of the Minpack license -// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#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 - }; -} - -template <typename _Scalar, int NX=Dynamic, int NY=Dynamic> -struct DenseFunctor -{ - typedef _Scalar Scalar; - enum { - InputsAtCompileTime = NX, - ValuesAtCompileTime = NY - }; - typedef Matrix<Scalar,InputsAtCompileTime,1> InputType; - typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType; - typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; - typedef ColPivHouseholderQR<JacobianType> QRSolver; - const int m_inputs, m_values; - - DenseFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} - DenseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {} - - int inputs() const { return m_inputs; } - int values() const { return m_values; } - - //int operator()(const InputType &x, ValueType& fvec) { } - // should be defined in derived classes - - //int df(const InputType &x, JacobianType& fjac) { } - // should be defined in derived classes -}; - -template <typename _Scalar, typename _Index> -struct SparseFunctor -{ - typedef _Scalar Scalar; - typedef _Index Index; - typedef Matrix<Scalar,Dynamic,1> InputType; - typedef Matrix<Scalar,Dynamic,1> ValueType; - typedef SparseMatrix<Scalar, ColMajor, Index> JacobianType; - typedef SparseQR<JacobianType, COLAMDOrdering<int> > QRSolver; - enum { - InputsAtCompileTime = Dynamic, - ValuesAtCompileTime = Dynamic - }; - - SparseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {} - - int inputs() const { return m_inputs; } - int values() const { return m_values; } - - const int m_inputs, m_values; - //int operator()(const InputType &x, ValueType& fvec) { } - // to be defined in the functor - - //int df(const InputType &x, JacobianType& fjac) { } - // to be defined in the functor if no automatic differentiation - -}; -namespace internal { -template <typename QRSolver, typename VectorType> -void lmpar2(const QRSolver &qr, const VectorType &diag, const VectorType &qtb, - typename VectorType::Scalar m_delta, typename VectorType::Scalar &par, - VectorType &x); - } -/** - * \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> -class LevenbergMarquardt : internal::no_assignment_operator -{ - public: - typedef _FunctorType FunctorType; - typedef typename FunctorType::QRSolver QRSolver; - typedef typename FunctorType::JacobianType JacobianType; - typedef typename JacobianType::Scalar Scalar; - typedef typename JacobianType::RealScalar RealScalar; - typedef typename QRSolver::StorageIndex PermIndex; - typedef Matrix<Scalar,Dynamic,1> FVectorType; - typedef PermutationMatrix<Dynamic,Dynamic> PermutationType; - public: - LevenbergMarquardt(FunctorType& functor) - : m_functor(functor),m_nfev(0),m_njev(0),m_fnorm(0.0),m_gnorm(0), - m_isInitialized(false),m_info(InvalidInput) - { - resetParameters(); - m_useExternalScaling=false; - } - - LevenbergMarquardtSpace::Status minimize(FVectorType &x); - LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x); - LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x); - LevenbergMarquardtSpace::Status lmder1( - FVectorType &x, - const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) - ); - static LevenbergMarquardtSpace::Status lmdif1( - FunctorType &functor, - FVectorType &x, - Index *nfev, - const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) - ); - - /** Sets the default parameters */ - void resetParameters() - { - using std::sqrt; - - m_factor = 100.; - m_maxfev = 400; - m_ftol = sqrt(NumTraits<RealScalar>::epsilon()); - m_xtol = sqrt(NumTraits<RealScalar>::epsilon()); - m_gtol = 0. ; - m_epsfcn = 0. ; - } - - /** Sets the tolerance for the norm of the solution vector*/ - void setXtol(RealScalar xtol) { m_xtol = xtol; } - - /** Sets the tolerance for the norm of the vector function*/ - void setFtol(RealScalar ftol) { m_ftol = ftol; } - - /** Sets the tolerance for the norm of the gradient of the error vector*/ - void setGtol(RealScalar gtol) { m_gtol = gtol; } - - /** Sets the step bound for the diagonal shift */ - void setFactor(RealScalar factor) { m_factor = factor; } - - /** Sets the error precision */ - void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; } - - /** Sets the maximum number of function evaluation */ - void setMaxfev(Index maxfev) {m_maxfev = maxfev; } - - /** Use an external Scaling. If set to true, pass a nonzero diagonal to diag() */ - void setExternalScaling(bool value) {m_useExternalScaling = value; } - - /** \returns the tolerance for the norm of the solution vector */ - RealScalar xtol() const {return m_xtol; } - - /** \returns the tolerance for the norm of the vector function */ - RealScalar ftol() const {return m_ftol; } - - /** \returns the tolerance for the norm of the gradient of the error vector */ - RealScalar gtol() const {return m_gtol; } - - /** \returns the step bound for the diagonal shift */ - RealScalar factor() const {return m_factor; } - - /** \returns the error precision */ - RealScalar epsilon() const {return m_epsfcn; } - - /** \returns the maximum number of function evaluation */ - Index maxfev() const {return m_maxfev; } - - /** \returns a reference to the diagonal of the jacobian */ - FVectorType& diag() {return m_diag; } - - /** \returns the number of iterations performed */ - Index iterations() { return m_iter; } - - /** \returns the number of functions evaluation */ - Index nfev() { return m_nfev; } - - /** \returns the number of jacobian evaluation */ - Index njev() { return m_njev; } - - /** \returns the norm of current vector function */ - RealScalar fnorm() {return m_fnorm; } - - /** \returns the norm of the gradient of the error */ - RealScalar gnorm() {return m_gnorm; } - - /** \returns the LevenbergMarquardt parameter */ - RealScalar lm_param(void) { return m_par; } - - /** \returns a reference to the current vector function - */ - FVectorType& fvec() {return m_fvec; } - - /** \returns a reference to the matrix where the current Jacobian matrix is stored - */ - JacobianType& jacobian() {return m_fjac; } - - /** \returns a reference to the triangular matrix R from the QR of the jacobian matrix. - * \sa jacobian() - */ - JacobianType& matrixR() {return m_rfactor; } - - /** the permutation used in the QR factorization - */ - PermutationType permutation() {return m_permutation; } - - /** - * \brief Reports whether the minimization was successful - * \returns \c Success if the minimization was succesful, - * \c NumericalIssue if a numerical problem arises during the - * minimization process, for exemple during the QR factorization - * \c NoConvergence if the minimization did not converge after - * the maximum number of function evaluation allowed - * \c InvalidInput if the input matrix is invalid - */ - ComputationInfo info() const - { - - return m_info; - } - private: - JacobianType m_fjac; - JacobianType m_rfactor; // The triangular matrix R from the QR of the jacobian matrix m_fjac - FunctorType &m_functor; - FVectorType m_fvec, m_qtf, m_diag; - Index n; - Index m; - Index m_nfev; - Index m_njev; - RealScalar m_fnorm; // Norm of the current vector function - RealScalar m_gnorm; //Norm of the gradient of the error - RealScalar m_factor; // - Index m_maxfev; // Maximum number of function evaluation - RealScalar m_ftol; //Tolerance in the norm of the vector function - RealScalar m_xtol; // - RealScalar m_gtol; //tolerance of the norm of the error gradient - RealScalar m_epsfcn; // - Index m_iter; // Number of iterations performed - RealScalar m_delta; - bool m_useExternalScaling; - PermutationType m_permutation; - FVectorType m_wa1, m_wa2, m_wa3, m_wa4; //Temporary vectors - RealScalar m_par; - bool m_isInitialized; // Check whether the minimization step has been called - ComputationInfo m_info; -}; - -template<typename FunctorType> -LevenbergMarquardtSpace::Status -LevenbergMarquardt<FunctorType>::minimize(FVectorType &x) -{ - LevenbergMarquardtSpace::Status status = minimizeInit(x); - if (status==LevenbergMarquardtSpace::ImproperInputParameters) { - m_isInitialized = true; - return status; - } - do { -// std::cout << " uv " << x.transpose() << "\n"; - status = minimizeOneStep(x); - } while (status==LevenbergMarquardtSpace::Running); - m_isInitialized = true; - return status; -} - -template<typename FunctorType> -LevenbergMarquardtSpace::Status -LevenbergMarquardt<FunctorType>::minimizeInit(FVectorType &x) -{ - n = x.size(); - m = m_functor.values(); - - m_wa1.resize(n); m_wa2.resize(n); m_wa3.resize(n); - m_wa4.resize(m); - m_fvec.resize(m); - //FIXME Sparse Case : Allocate space for the jacobian - m_fjac.resize(m, n); -// m_fjac.reserve(VectorXi::Constant(n,5)); // FIXME Find a better alternative - if (!m_useExternalScaling) - m_diag.resize(n); - eigen_assert( (!m_useExternalScaling || m_diag.size()==n) && "When m_useExternalScaling is set, the caller must provide a valid 'm_diag'"); - m_qtf.resize(n); - - /* Function Body */ - m_nfev = 0; - m_njev = 0; - - /* check the input parameters for errors. */ - if (n <= 0 || m < n || m_ftol < 0. || m_xtol < 0. || m_gtol < 0. || m_maxfev <= 0 || m_factor <= 0.){ - m_info = InvalidInput; - return LevenbergMarquardtSpace::ImproperInputParameters; - } - - if (m_useExternalScaling) - for (Index j = 0; j < n; ++j) - if (m_diag[j] <= 0.) - { - m_info = InvalidInput; - return LevenbergMarquardtSpace::ImproperInputParameters; - } - - /* evaluate the function at the starting point */ - /* and calculate its norm. */ - m_nfev = 1; - if ( m_functor(x, m_fvec) < 0) - return LevenbergMarquardtSpace::UserAsked; - m_fnorm = m_fvec.stableNorm(); - - /* initialize levenberg-marquardt parameter and iteration counter. */ - m_par = 0.; - m_iter = 1; - - return LevenbergMarquardtSpace::NotStarted; -} - -template<typename FunctorType> -LevenbergMarquardtSpace::Status -LevenbergMarquardt<FunctorType>::lmder1( - FVectorType &x, - const Scalar tol - ) -{ - n = x.size(); - m = m_functor.values(); - - /* check the input parameters for errors. */ - if (n <= 0 || m < n || tol < 0.) - return LevenbergMarquardtSpace::ImproperInputParameters; - - resetParameters(); - m_ftol = tol; - m_xtol = tol; - m_maxfev = 100*(n+1); - - return minimize(x); -} - - -template<typename FunctorType> -LevenbergMarquardtSpace::Status -LevenbergMarquardt<FunctorType>::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> > lm(numDiff); - lm.setFtol(tol); - lm.setXtol(tol); - lm.setMaxfev(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 |