diff options
Diffstat (limited to 'unsupported/Eigen')
| -rw-r--r-- | unsupported/Eigen/CMakeLists.txt | 2 | ||||
| -rw-r--r-- | unsupported/Eigen/LevenbergMarquardt | 46 | ||||
| -rw-r--r-- | unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt | 6 | ||||
| -rw-r--r-- | unsupported/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt | 52 | ||||
| -rw-r--r-- | unsupported/Eigen/src/LevenbergMarquardt/LMcovar.h | 85 | ||||
| -rw-r--r-- | unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h | 179 | ||||
| -rw-r--r-- | unsupported/Eigen/src/LevenbergMarquardt/LMpar.h | 160 | ||||
| -rw-r--r-- | unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h | 189 | ||||
| -rw-r--r-- | unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h | 344 |
9 files changed, 1062 insertions, 1 deletions
diff --git a/unsupported/Eigen/CMakeLists.txt b/unsupported/Eigen/CMakeLists.txt index e961e72c5..e06f1238b 100644 --- a/unsupported/Eigen/CMakeLists.txt +++ b/unsupported/Eigen/CMakeLists.txt @@ -1,6 +1,6 @@ set(Eigen_HEADERS AdolcForward BVH IterativeSolvers MatrixFunctions MoreVectorization AutoDiff AlignedVector3 Polynomials FFT NonLinearOptimization SparseExtra IterativeSolvers - NumericalDiff Skyline MPRealSupport OpenGLSupport KroneckerProduct Splines + NumericalDiff Skyline MPRealSupport OpenGLSupport KroneckerProduct Splines LevenbergMarquardt ) install(FILES diff --git a/unsupported/Eigen/LevenbergMarquardt b/unsupported/Eigen/LevenbergMarquardt new file mode 100644 index 000000000..5ce1a23f6 --- /dev/null +++ b/unsupported/Eigen/LevenbergMarquardt @@ -0,0 +1,46 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_LEVENBERGMARQUARDT_MODULE +#define EIGEN_LEVENBERGMARQUARDT_MODULE + +// #include <vector> + +#include <Eigen/Core> +#include <Eigen/Jacobi> +#include <Eigen/QR> +#include <unsupported/Eigen/NumericalDiff> +#ifdef EIGEN_SPQR_SUPPORT +#include <Eigen/SPQRSupport> +#endif + +/** \ingroup NonLinearOptimization_modules + * \defgroup LevenbergMarquardt_Module Levenberg-Marquardt optimization module + * + * \code + * #include </Eigen/LevenbergMarquardt> + * \endcode + * + * + */ + +#include "Eigen/SparseCore" +#ifndef EIGEN_PARSED_BY_DOXYGEN + +#include "src/LevenbergMarquardt/LMqrsolv.h" +#include "src/LevenbergMarquardt/LMcovar.h" +#include "src/LevenbergMarquardt/LMpar.h" + +#endif + +#include "src/LevenbergMarquardt/LevenbergMarquardt.h" +#include "src/LevenbergMarquardt/LMonestep.h" + + +#endif // EIGEN_LEVENBERGMARQUARDT_MODULE diff --git a/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt b/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt new file mode 100644 index 000000000..8513803ce --- /dev/null +++ b/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_LevenbergMarquardt_SRCS "*.h") + +INSTALL(FILES + ${Eigen_LevenbergMarquardt_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/LevenbergMarquardt COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt b/unsupported/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt new file mode 100644 index 000000000..ae7984dae --- /dev/null +++ b/unsupported/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt @@ -0,0 +1,52 @@ +Minpack Copyright Notice (1999) University of Chicago. All rights reserved + +Redistribution and use in source and binary forms, with or +without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above +copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above +copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials +provided with the distribution. + +3. The end-user documentation included with the +redistribution, if any, must include the following +acknowledgment: + + "This product includes software developed by the + University of Chicago, as Operator of Argonne National + Laboratory. + +Alternately, this acknowledgment may appear in the software +itself, if and wherever such third-party acknowledgments +normally appear. + +4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS" +WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE +UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND +THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES +OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE +OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY +OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR +USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF +THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4) +DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION +UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL +BE CORRECTED. + +5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT +HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF +ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT, +INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF +ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF +PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER +SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT +(INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE, +EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE +POSSIBILITY OF SUCH LOSS OR DAMAGES. + diff --git a/unsupported/Eigen/src/LevenbergMarquardt/LMcovar.h b/unsupported/Eigen/src/LevenbergMarquardt/LMcovar.h new file mode 100644 index 000000000..3210587e4 --- /dev/null +++ b/unsupported/Eigen/src/LevenbergMarquardt/LMcovar.h @@ -0,0 +1,85 @@ +// 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; + 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); + 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/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h b/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h new file mode 100644 index 000000000..aa0c02668 --- /dev/null +++ b/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h @@ -0,0 +1,179 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> +// +// This code initially comes from MINPACK whose original authors are: +// Copyright Jorge More - Argonne National Laboratory +// Copyright Burt Garbow - Argonne National Laboratory +// Copyright Ken Hillstrom - Argonne National Laboratory +// +// This Source Code Form is subject to the terms of the Minpack license +// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. + +#ifndef EIGEN_LMONESTEP_H +#define EIGEN_LMONESTEP_H + +namespace Eigen { + +template<typename FunctorType> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType>::minimizeOneStep(FVectorType &x) +{ + typedef typename FunctorType::JacobianType JacobianType; + using std::abs; + using std::sqrt; + RealScalar temp, temp1,temp2; + RealScalar ratio; + RealScalar pnorm, xnorm, fnorm1, actred, dirder, prered; + assert(x.size()==n); // check the caller is not cheating us + + temp = 0.0; xnorm = 0.0; + /* calculate the jacobian matrix. */ + Index df_ret = m_functor.df(x, m_fjac); + if (df_ret<0) + return LevenbergMarquardtSpace::UserAsked; + if (df_ret>0) + // numerical diff, we evaluated the function df_ret times + m_nfev += df_ret; + else m_njev++; + + /* compute the qr factorization of the jacobian. */ + for (int j = 0; j < x.size(); ++j) + m_wa2(j) = m_fjac.col(j).norm(); + //FIXME Implement bluenorm for sparse vectors +// m_wa2 = m_fjac.colwise().blueNorm(); + QRSolver qrfac(m_fjac); //FIXME Check if the QR decomposition succeed + // Make a copy of the first factor with the associated permutation + JacobianType rfactor; + rfactor = qrfac.matrixQR(); + m_permutation = (qrfac.colsPermutation()); + + /* on the first iteration and if external scaling is not used, scale according */ + /* to the norms of the columns of the initial jacobian. */ + if (m_iter == 1) { + if (!m_useExternalScaling) + for (Index j = 0; j < n; ++j) + m_diag[j] = (m_wa2[j]==0.)? 1. : m_wa2[j]; + + /* on the first iteration, calculate the norm of the scaled x */ + /* and initialize the step bound m_delta. */ + xnorm = m_diag.cwiseProduct(x).stableNorm(); + m_delta = m_factor * xnorm; + if (m_delta == 0.) + m_delta = m_factor; + } + + /* form (q transpose)*m_fvec and store the first n components in */ + /* m_qtf. */ + m_wa4 = m_fvec; + m_wa4 = qrfac.matrixQ().adjoint() * m_fvec; + m_qtf = m_wa4.head(n); + + /* compute the norm of the scaled gradient. */ + m_gnorm = 0.; + if (m_fnorm != 0.) + for (Index j = 0; j < n; ++j) + if (m_wa2[m_permutation.indices()[j]] != 0.) + m_gnorm = (std::max)(m_gnorm, abs( rfactor.col(j).head(j+1).dot(m_qtf.head(j+1)/m_fnorm) / m_wa2[m_permutation.indices()[j]])); + + /* test for convergence of the gradient norm. */ + if (m_gnorm <= m_gtol) + return LevenbergMarquardtSpace::CosinusTooSmall; + + /* rescale if necessary. */ + if (!m_useExternalScaling) + m_diag = m_diag.cwiseMax(m_wa2); + + do { + /* determine the levenberg-marquardt parameter. */ + internal::lmpar2(qrfac, m_diag, m_qtf, m_delta, m_par, m_wa1); + + /* store the direction p and x + p. calculate the norm of p. */ + m_wa1 = -m_wa1; + m_wa2 = x + m_wa1; + pnorm = m_diag.cwiseProduct(m_wa1).stableNorm(); + + /* on the first iteration, adjust the initial step bound. */ + if (m_iter == 1) + m_delta = (std::min)(m_delta,pnorm); + + /* evaluate the function at x + p and calculate its norm. */ + if ( m_functor(m_wa2, m_wa4) < 0) + return LevenbergMarquardtSpace::UserAsked; + ++m_nfev; + fnorm1 = m_wa4.stableNorm(); + + /* compute the scaled actual reduction. */ + actred = -1.; + if (Scalar(.1) * fnorm1 < m_fnorm) + actred = 1. - internal::abs2(fnorm1 / m_fnorm); + + /* compute the scaled predicted reduction and */ + /* the scaled directional derivative. */ + m_wa3 = rfactor.template triangularView<Upper>() * (m_permutation.inverse() *m_wa1); + temp1 = internal::abs2(m_wa3.stableNorm() / m_fnorm); + temp2 = internal::abs2(sqrt(m_par) * pnorm / m_fnorm); + prered = temp1 + temp2 / Scalar(.5); + dirder = -(temp1 + temp2); + + /* compute the ratio of the actual to the predicted */ + /* reduction. */ + ratio = 0.; + if (prered != 0.) + ratio = actred / prered; + + /* update the step bound. */ + if (ratio <= Scalar(.25)) { + if (actred >= 0.) + temp = RealScalar(.5); + if (actred < 0.) + temp = RealScalar(.5) * dirder / (dirder + RealScalar(.5) * actred); + if (RealScalar(.1) * fnorm1 >= m_fnorm || temp < RealScalar(.1)) + temp = Scalar(.1); + /* Computing MIN */ + m_delta = temp * (std::min)(m_delta, pnorm / RealScalar(.1)); + m_par /= temp; + } else if (!(m_par != 0. && ratio < RealScalar(.75))) { + m_delta = pnorm / RealScalar(.5); + m_par = RealScalar(.5) * m_par; + } + + /* test for successful iteration. */ + if (ratio >= RealScalar(1e-4)) { + /* successful iteration. update x, m_fvec, and their norms. */ + x = m_wa2; + m_wa2 = m_diag.cwiseProduct(x); + m_fvec = m_wa4; + xnorm = m_wa2.stableNorm(); + m_fnorm = fnorm1; + ++m_iter; + } + + /* tests for convergence. */ + if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1. && m_delta <= m_xtol * xnorm) + return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall; + if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1.) + return LevenbergMarquardtSpace::RelativeReductionTooSmall; + if (m_delta <= m_xtol * xnorm) + return LevenbergMarquardtSpace::RelativeErrorTooSmall; + + /* tests for termination and stringent tolerances. */ + if (m_nfev >= m_maxfev) + return LevenbergMarquardtSpace::TooManyFunctionEvaluation; + if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.) + return LevenbergMarquardtSpace::FtolTooSmall; + if (m_delta <= NumTraits<Scalar>::epsilon() * xnorm) + return LevenbergMarquardtSpace::XtolTooSmall; + if (m_gnorm <= NumTraits<Scalar>::epsilon()) + return LevenbergMarquardtSpace::GtolTooSmall; + + } while (ratio < Scalar(1e-4)); + + return LevenbergMarquardtSpace::Running; +} + + +} // end namespace Eigen + +#endif // EIGEN_LMONESTEP_H
\ No newline at end of file diff --git a/unsupported/Eigen/src/LevenbergMarquardt/LMpar.h b/unsupported/Eigen/src/LevenbergMarquardt/LMpar.h new file mode 100644 index 000000000..dc60ca05a --- /dev/null +++ b/unsupported/Eigen/src/LevenbergMarquardt/LMpar.h @@ -0,0 +1,160 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This code initially comes from MINPACK whose original authors are: +// Copyright Jorge More - Argonne National Laboratory +// Copyright Burt Garbow - Argonne National Laboratory +// Copyright Ken Hillstrom - Argonne National Laboratory +// +// This Source Code Form is subject to the terms of the Minpack license +// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. + +#ifndef EIGEN_LMPAR_H +#define EIGEN_LMPAR_H + +namespace Eigen { + +namespace internal { + + template <typename QRSolver, typename VectorType> + void lmpar2( + const QRSolver &qr, + const VectorType &diag, + const VectorType &qtb, + typename VectorType::Scalar m_delta, + typename VectorType::Scalar &par, + VectorType &x) + + { + using std::sqrt; + using std::abs; + typedef typename QRSolver::MatrixType MatrixType; + typedef typename QRSolver::Scalar Scalar; + typedef typename QRSolver::Index Index; + + /* Local variables */ + Index j; + Scalar fp; + Scalar parc, parl; + Index iter; + Scalar temp, paru; + Scalar gnorm; + Scalar dxnorm; + + + /* Function Body */ + const Scalar dwarf = (std::numeric_limits<Scalar>::min)(); + const Index n = qr.matrixQR().cols(); + assert(n==diag.size()); + assert(n==qtb.size()); + + VectorType wa1, wa2; + + /* compute and store in x the gauss-newton direction. if the */ + /* jacobian is rank-deficient, obtain a least squares solution. */ + + // const Index rank = qr.nonzeroPivots(); // exactly double(0.) + const Index rank = qr.rank(); // use a threshold + wa1 = qtb; + wa1.tail(n-rank).setZero(); + //FIXME There is no solve in place for sparse triangularView + //qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank)); + wa1.head(rank) = qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solve(qtb.head(rank)); + + x = qr.colsPermutation()*wa1; + + /* initialize the iteration counter. */ + /* evaluate the function at the origin, and test */ + /* for acceptance of the gauss-newton direction. */ + iter = 0; + wa2 = diag.cwiseProduct(x); + dxnorm = wa2.blueNorm(); + fp = dxnorm - m_delta; + if (fp <= Scalar(0.1) * m_delta) { + par = 0; + return; + } + + /* if the jacobian is not rank deficient, the newton */ + /* step provides a lower bound, parl, for the zero of */ + /* the function. otherwise set this bound to zero. */ + parl = 0.; + if (rank==n) { + wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm; + qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1); + temp = wa1.blueNorm(); + parl = fp / m_delta / temp / temp; + } + + /* calculate an upper bound, paru, for the zero of the function. */ + for (j = 0; j < n; ++j) + wa1[j] = qr.matrixQR().col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)]; + + gnorm = wa1.stableNorm(); + paru = gnorm / m_delta; + if (paru == 0.) + paru = dwarf / (std::min)(m_delta,Scalar(0.1)); + + /* if the input par lies outside of the interval (parl,paru), */ + /* set par to the closer endpoint. */ + par = (std::max)(par,parl); + par = (std::min)(par,paru); + if (par == 0.) + par = gnorm / dxnorm; + + /* beginning of an iteration. */ + MatrixType s; + s = qr.matrixQR(); + while (true) { + ++iter; + + /* evaluate the function at the current value of par. */ + if (par == 0.) + par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ + wa1 = sqrt(par)* diag; + + VectorType sdiag(n); + lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag); + + wa2 = diag.cwiseProduct(x); + dxnorm = wa2.blueNorm(); + temp = fp; + fp = dxnorm - m_delta; + + /* if the function is small enough, accept the current value */ + /* of par. also test for the exceptional cases where parl */ + /* is zero or the number of iterations has reached 10. */ + if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) + break; + + /* compute the newton correction. */ + wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm); + // we could almost use this here, but the diagonal is outside qr, in sdiag[] + // qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1); + for (j = 0; j < n; ++j) { + wa1[j] /= sdiag[j]; + temp = wa1[j]; + for (Index i = j+1; i < n; ++i) + wa1[i] -= s.coeff(i,j) * temp; + } + temp = wa1.blueNorm(); + parc = fp / m_delta / temp / temp; + + /* depending on the sign of the function, update parl or paru. */ + if (fp > 0.) + parl = (std::max)(parl,par); + if (fp < 0.) + paru = (std::min)(paru,par); + + /* compute an improved estimate for par. */ + par = (std::max)(parl,par+parc); + } + if (iter == 0) + par = 0.; + return; + } +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_LMPAR_H diff --git a/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h b/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h new file mode 100644 index 000000000..ed6f97fe8 --- /dev/null +++ b/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h @@ -0,0 +1,189 @@ +// 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 SizeAtCompileTime, typename Index> +void lmqrsolv( + Matrix<Scalar,SizeAtCompileTime,SizeAtCompileTime> &s, + const PermutationMatrix<Dynamic,Dynamic,Index> &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, 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 = 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/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h b/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h new file mode 100644 index 000000000..e45e73ab5 --- /dev/null +++ b/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h @@ -0,0 +1,344 @@ +// 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 +}; + +#ifdef EIGEN_SPQR_SUPPORT +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 SPQR<JacobianType> QRSolver; + + 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 + +}; +#endif +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 +{ + 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 JacobianType::Index Index; + typedef typename QRSolver::Index 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) + { + 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() + { + m_factor = 100.; + m_maxfev = 400; + m_ftol = std::sqrt(NumTraits<RealScalar>::epsilon()); + m_xtol = std::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; } + + /** Get a reference to the diagonal of the jacobian */ + FVectorType& diag() {return m_diag; } + + /** Number of iterations performed */ + Index iterations() { return m_iter; } + + /** Number of functions evaluation */ + Index nfev() { return m_nfev; } + + /** Number of jacobian evaluation */ + Index njev() { return m_njev; } + + /** Norm of current vector function */ + RealScalar fnorm() {return m_fnorm; } + + /** Norm of the gradient of the error */ + RealScalar gnorm() {return m_gnorm; } + + /** the LevenbergMarquardt parameter */ + RealScalar lm_param(void) { return m_par; } + + /** reference to the current vector function + */ + FVectorType& fvec() {return m_fvec; } + + /** reference to the matrix where the current Jacobian matrix is stored + */ + JacobianType& fjac() {return m_fjac; } + + /** the permutation used + */ + PermutationType permutation() {return m_permutation; } + + private: + JacobianType 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; +}; + +template<typename FunctorType> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType>::minimize(FVectorType &x) +{ + LevenbergMarquardtSpace::Status status = minimizeInit(x); + if (status==LevenbergMarquardtSpace::ImproperInputParameters) + return status; + do { +// std::cout << " uv " << x.transpose() << "\n"; + status = minimizeOneStep(x); + } while (status==LevenbergMarquardtSpace::Running); + 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); + 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.) + return LevenbergMarquardtSpace::ImproperInputParameters; + + if (m_useExternalScaling) + for (Index j = 0; j < n; ++j) + if (m_diag[j] <= 0.) + 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 |