1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // This code initially comes from MINPACK whose original authors are: 5 // Copyright Jorge More - Argonne National Laboratory 6 // Copyright Burt Garbow - Argonne National Laboratory 7 // Copyright Ken Hillstrom - Argonne National Laboratory 8 // 9 // This Source Code Form is subject to the terms of the Minpack license 10 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. 11 12 #ifndef EIGEN_LMPAR_H 13 #define EIGEN_LMPAR_H 14 15 namespace Eigen { 16 17 namespace internal { 18 19 template <typename QRSolver, typename VectorType> 20 void lmpar2( 21 const QRSolver &qr, 22 const VectorType &diag, 23 const VectorType &qtb, 24 typename VectorType::Scalar m_delta, 25 typename VectorType::Scalar &par, 26 VectorType &x) 27 28 { 29 using std::sqrt; 30 using std::abs; 31 typedef typename QRSolver::MatrixType MatrixType; 32 typedef typename QRSolver::Scalar Scalar; 33 // typedef typename QRSolver::StorageIndex StorageIndex; 34 35 /* Local variables */ 36 Index j; 37 Scalar fp; 38 Scalar parc, parl; 39 Index iter; 40 Scalar temp, paru; 41 Scalar gnorm; 42 Scalar dxnorm; 43 44 // Make a copy of the triangular factor. 45 // This copy is modified during call the qrsolv 46 MatrixType s; 47 s = qr.matrixR(); 48 49 /* Function Body */ 50 const Scalar dwarf = (std::numeric_limits<Scalar>::min)(); 51 const Index n = qr.matrixR().cols(); 52 eigen_assert(n==diag.size()); 53 eigen_assert(n==qtb.size()); 54 55 VectorType wa1, wa2; 56 57 /* compute and store in x the gauss-newton direction. if the */ 58 /* jacobian is rank-deficient, obtain a least squares solution. */ 59 60 // const Index rank = qr.nonzeroPivots(); // exactly double(0.) 61 const Index rank = qr.rank(); // use a threshold 62 wa1 = qtb; 63 wa1.tail(n-rank).setZero(); 64 //FIXME There is no solve in place for sparse triangularView 65 wa1.head(rank) = s.topLeftCorner(rank,rank).template triangularView<Upper>().solve(qtb.head(rank)); 66 67 x = qr.colsPermutation()*wa1; 68 69 /* initialize the iteration counter. */ 70 /* evaluate the function at the origin, and test */ 71 /* for acceptance of the gauss-newton direction. */ 72 iter = 0; 73 wa2 = diag.cwiseProduct(x); 74 dxnorm = wa2.blueNorm(); 75 fp = dxnorm - m_delta; 76 if (fp <= Scalar(0.1) * m_delta) { 77 par = 0; 78 return; 79 } 80 81 /* if the jacobian is not rank deficient, the newton */ 82 /* step provides a lower bound, parl, for the zero of */ 83 /* the function. otherwise set this bound to zero. */ 84 parl = 0.; 85 if (rank==n) { 86 wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm; 87 s.topLeftCorner(n,n).transpose().template triangularView<Lower>().solveInPlace(wa1); 88 temp = wa1.blueNorm(); 89 parl = fp / m_delta / temp / temp; 90 } 91 92 /* calculate an upper bound, paru, for the zero of the function. */ 93 for (j = 0; j < n; ++j) 94 wa1[j] = s.col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)]; 95 96 gnorm = wa1.stableNorm(); 97 paru = gnorm / m_delta; 98 if (paru == 0.) 99 paru = dwarf / (std::min)(m_delta,Scalar(0.1)); 100 101 /* if the input par lies outside of the interval (parl,paru), */ 102 /* set par to the closer endpoint. */ 103 par = (std::max)(par,parl); 104 par = (std::min)(par,paru); 105 if (par == 0.) 106 par = gnorm / dxnorm; 107 108 /* beginning of an iteration. */ 109 while (true) { 110 ++iter; 111 112 /* evaluate the function at the current value of par. */ 113 if (par == 0.) 114 par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ 115 wa1 = sqrt(par)* diag; 116 117 VectorType sdiag(n); 118 lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag); 119 120 wa2 = diag.cwiseProduct(x); 121 dxnorm = wa2.blueNorm(); 122 temp = fp; 123 fp = dxnorm - m_delta; 124 125 /* if the function is small enough, accept the current value */ 126 /* of par. also test for the exceptional cases where parl */ 127 /* is zero or the number of iterations has reached 10. */ 128 if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) 129 break; 130 131 /* compute the newton correction. */ 132 wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm); 133 // we could almost use this here, but the diagonal is outside qr, in sdiag[] 134 for (j = 0; j < n; ++j) { 135 wa1[j] /= sdiag[j]; 136 temp = wa1[j]; 137 for (Index i = j+1; i < n; ++i) 138 wa1[i] -= s.coeff(i,j) * temp; 139 } 140 temp = wa1.blueNorm(); 141 parc = fp / m_delta / temp / temp; 142 143 /* depending on the sign of the function, update parl or paru. */ 144 if (fp > 0.) 145 parl = (std::max)(parl,par); 146 if (fp < 0.) 147 paru = (std::min)(paru,par); 148 149 /* compute an improved estimate for par. */ 150 par = (std::max)(parl,par+parc); 151 } 152 if (iter == 0) 153 par = 0.; 154 return; 155 } 156 } // end namespace internal 157 158 } // end namespace Eigen 159 160 #endif // EIGEN_LMPAR_H 161
