blob: 74acc9a960d7223ace1020ce9fa0410db2eb4bfd [file] [log] [blame]
// 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
#include "./InternalHeaderCheck.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