17faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// This file is part of Eigen, a lightweight C++ template library 27faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// for linear algebra. 37faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// 47faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// This code initially comes from MINPACK whose original authors are: 57faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright Jorge More - Argonne National Laboratory 67faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright Burt Garbow - Argonne National Laboratory 77faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright Ken Hillstrom - Argonne National Laboratory 87faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// 97faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// This Source Code Form is subject to the terms of the Minpack license 107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. 117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#ifndef EIGEN_LMPAR_H 137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#define EIGEN_LMPAR_H 147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeznamespace Eigen { 167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeznamespace internal { 187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez template <typename QRSolver, typename VectorType> 207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez void lmpar2( 217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const QRSolver &qr, 227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const VectorType &diag, 237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const VectorType &qtb, 247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typename VectorType::Scalar m_delta, 257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typename VectorType::Scalar &par, 267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType &x) 277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::sqrt; 307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::abs; 317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename QRSolver::MatrixType MatrixType; 327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename QRSolver::Scalar Scalar; 337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename QRSolver::Index Index; 347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* Local variables */ 367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index j; 377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar fp; 387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar parc, parl; 397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index iter; 407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar temp, paru; 417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar gnorm; 427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar dxnorm; 437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Make a copy of the triangular factor. 457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // This copy is modified during call the qrsolv 467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez MatrixType s; 477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez s = qr.matrixR(); 487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* Function Body */ 507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Scalar dwarf = (std::numeric_limits<Scalar>::min)(); 517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Index n = qr.matrixR().cols(); 527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(n==diag.size()); 537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(n==qtb.size()); 547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType wa1, wa2; 567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* compute and store in x the gauss-newton direction. if the */ 587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* jacobian is rank-deficient, obtain a least squares solution. */ 597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // const Index rank = qr.nonzeroPivots(); // exactly double(0.) 617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Index rank = qr.rank(); // use a threshold 627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa1 = qtb; 637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa1.tail(n-rank).setZero(); 647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //FIXME There is no solve in place for sparse triangularView 657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa1.head(rank) = s.topLeftCorner(rank,rank).template triangularView<Upper>().solve(qtb.head(rank)); 667faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez x = qr.colsPermutation()*wa1; 687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* initialize the iteration counter. */ 707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* evaluate the function at the origin, and test */ 717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* for acceptance of the gauss-newton direction. */ 727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez iter = 0; 737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa2 = diag.cwiseProduct(x); 747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez dxnorm = wa2.blueNorm(); 757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez fp = dxnorm - m_delta; 767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (fp <= Scalar(0.1) * m_delta) { 777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez par = 0; 787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return; 797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* if the jacobian is not rank deficient, the newton */ 827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* step provides a lower bound, parl, for the zero of */ 837faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* the function. otherwise set this bound to zero. */ 847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez parl = 0.; 857faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (rank==n) { 867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm; 877faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez s.topLeftCorner(n,n).transpose().template triangularView<Lower>().solveInPlace(wa1); 887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp = wa1.blueNorm(); 897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez parl = fp / m_delta / temp / temp; 907faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* calculate an upper bound, paru, for the zero of the function. */ 937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (j = 0; j < n; ++j) 947faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa1[j] = s.col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)]; 957faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 967faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez gnorm = wa1.stableNorm(); 977faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez paru = gnorm / m_delta; 987faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (paru == 0.) 997faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez paru = dwarf / (std::min)(m_delta,Scalar(0.1)); 1007faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1017faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* if the input par lies outside of the interval (parl,paru), */ 1027faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* set par to the closer endpoint. */ 1037faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez par = (std::max)(par,parl); 1047faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez par = (std::min)(par,paru); 1057faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (par == 0.) 1067faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez par = gnorm / dxnorm; 1077faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1087faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* beginning of an iteration. */ 1097faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez while (true) { 1107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ++iter; 1117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* evaluate the function at the current value of par. */ 1137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (par == 0.) 1147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ 1157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa1 = sqrt(par)* diag; 1167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType sdiag(n); 1187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag); 1197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa2 = diag.cwiseProduct(x); 1217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez dxnorm = wa2.blueNorm(); 1227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp = fp; 1237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez fp = dxnorm - m_delta; 1247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* if the function is small enough, accept the current value */ 1267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* of par. also test for the exceptional cases where parl */ 1277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* is zero or the number of iterations has reached 10. */ 1287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) 1297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez break; 1307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* compute the newton correction. */ 1327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm); 1337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // we could almost use this here, but the diagonal is outside qr, in sdiag[] 1347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (j = 0; j < n; ++j) { 1357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa1[j] /= sdiag[j]; 1367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp = wa1[j]; 1377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (Index i = j+1; i < n; ++i) 1387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez wa1[i] -= s.coeff(i,j) * temp; 1397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp = wa1.blueNorm(); 1417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez parc = fp / m_delta / temp / temp; 1427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* depending on the sign of the function, update parl or paru. */ 1447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (fp > 0.) 1457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez parl = (std::max)(parl,par); 1467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (fp < 0.) 1477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez paru = (std::min)(paru,par); 1487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* compute an improved estimate for par. */ 1507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez par = (std::max)(parl,par+parc); 1517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (iter == 0) 1537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez par = 0.; 1547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return; 1557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} // end namespace internal 1577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} // end namespace Eigen 1597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#endif // EIGEN_LMPAR_H 161