devtools/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2026-04-10 11:34:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move work in progress Levenberg Marquardt module in unsupported

Browse Source
This commit is contained in:
Gael Guennebaud
2012-12-08 18:22:23 +01:00
parent 4cdcb6d793
commit 8844f632fa
12 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
Eigen/LevenbergMarquardt
View File

@@ -1,46 +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 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

6
Eigen/src/LevenbergMarquardt/CMakeLists.txt
View File

@@ -1,6 +0,0 @@
FILE(GLOB Eigen_LevenbergMarquardt_SRCS "*.h")
INSTALL(FILES
${Eigen_LevenbergMarquardt_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/LevenbergMarquardt COMPONENT Devel
)

52
Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt
View File

@@ -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.

85
Eigen/src/LevenbergMarquardt/LMcovar.h
View File

@@ -1,85 +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;
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

179
Eigen/src/LevenbergMarquardt/LMonestep.h
View File

@@ -1,179 +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)
{
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

160
Eigen/src/LevenbergMarquardt/LMpar.h
View File

@@ -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::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

189
Eigen/src/LevenbergMarquardt/LMqrsolv.h
View File

@@ -1,189 +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 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

344
Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h
View File

@@ -1,344 +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
};
#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