1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace Eigen { 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace internal { 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <typename Scalar> 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathvoid lmpar( 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix< Scalar, Dynamic, Dynamic > &r, 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const VectorXi &ipvt, 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Matrix< Scalar, Dynamic, 1 > &diag, 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Matrix< Scalar, Dynamic, 1 > &qtb, 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar delta, 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar &par, 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix< Scalar, Dynamic, 1 > &x) 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::abs; 167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::sqrt; 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef DenseIndex Index; 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Local variables */ 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index i, j, l; 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar fp; 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar parc, parl; 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index iter; 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar temp, paru; 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar gnorm; 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar dxnorm; 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Function Body */ 307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Scalar dwarf = (std::numeric_limits<Scalar>::min)(); 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index n = r.cols(); 327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(n==diag.size()); 337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(n==qtb.size()); 347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(n==x.size()); 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix< Scalar, Dynamic, 1 > wa1, wa2; 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute and store in x the gauss-newton direction. if the */ 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* jacobian is rank-deficient, obtain a least squares solution. */ 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index nsing = n-1; 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = qtb; 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) { 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (r(j,j) == 0. && nsing == n-1) 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath nsing = j - 1; 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (nsing < n-1) 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[j] = 0.; 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = nsing; j>=0; --j) { 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[j] /= r(j,j); 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = wa1[j]; 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (i = 0; i < j ; ++i) 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[i] -= r(i,j) * temp; 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath x[ipvt[j]] = wa1[j]; 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* initialize the iteration counter. */ 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* evaluate the function at the origin, and test */ 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* for acceptance of the gauss-newton direction. */ 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath iter = 0; 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = diag.cwiseProduct(x); 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath dxnorm = wa2.blueNorm(); 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fp = dxnorm - delta; 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fp <= Scalar(0.1) * delta) { 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = 0; 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return; 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* if the jacobian is not rank deficient, the newton */ 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* step provides a lower bound, parl, for the zero of */ 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* the function. otherwise set this bound to zero. */ 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parl = 0.; 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (nsing >= n-1) { 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) { 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath l = ipvt[j]; 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[j] = diag[l] * (wa2[l] / dxnorm); 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // it's actually a triangularView.solveInplace(), though in a weird 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // way: 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) { 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar sum = 0.; 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (i = 0; i < j; ++i) 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath sum += r(i,j) * wa1[i]; 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[j] = (wa1[j] - sum) / r(j,j); 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = wa1.blueNorm(); 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parl = fp / delta / temp / temp; 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* calculate an upper bound, paru, for the zero of the function. */ 92c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[j] = r.col(j).head(j+1).dot(qtb.head(j+1)) / diag[ipvt[j]]; 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath gnorm = wa1.stableNorm(); 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath paru = gnorm / delta; 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (paru == 0.) 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath paru = dwarf / (std::min)(delta,Scalar(0.1)); 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* if the input par lies outside of the interval (parl,paru), */ 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* set par to the closer endpoint. */ 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = (std::max)(par,parl); 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = (std::min)(par,paru); 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (par == 0.) 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = gnorm / dxnorm; 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* beginning of an iteration. */ 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath while (true) { 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++iter; 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* evaluate the function at the current value of par. */ 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (par == 0.) 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = sqrt(par)* diag; 115c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix< Scalar, Dynamic, 1 > sdiag(n); 117c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag); 118c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 119c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = diag.cwiseProduct(x); 120c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath dxnorm = wa2.blueNorm(); 121c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = fp; 122c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fp = dxnorm - delta; 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* if the function is small enough, accept the current value */ 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* of par. also test for the exceptional cases where parl */ 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* is zero or the number of iterations has reached 10. */ 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath break; 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 130c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the newton correction. */ 131c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) { 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath l = ipvt[j]; 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[j] = diag[l] * (wa2[l] / dxnorm); 134c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) { 136c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[j] /= sdiag[j]; 137c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = wa1[j]; 138c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (i = j+1; i < n; ++i) 139c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[i] -= r(i,j) * temp; 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = wa1.blueNorm(); 142c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parc = fp / delta / temp / temp; 143c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 144c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* depending on the sign of the function, update parl or paru. */ 145c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fp > 0.) 146c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parl = (std::max)(parl,par); 147c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fp < 0.) 148c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath paru = (std::min)(paru,par); 149c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 150c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute an improved estimate for par. */ 151c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Computing MAX */ 152c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = (std::max)(parl,par+parc); 153c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 154c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* end of an iteration. */ 155c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 156c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 157c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* termination. */ 158c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (iter == 0) 159c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = 0.; 160c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return; 161c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 162c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 163c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <typename Scalar> 164c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathvoid lmpar2( 165c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const ColPivHouseholderQR<Matrix< Scalar, Dynamic, Dynamic> > &qr, 166c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Matrix< Scalar, Dynamic, 1 > &diag, 167c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Matrix< Scalar, Dynamic, 1 > &qtb, 168c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar delta, 169c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar &par, 170c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix< Scalar, Dynamic, 1 > &x) 171c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 172c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 1737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::sqrt; 1747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::abs; 175c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef DenseIndex Index; 176c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 177c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Local variables */ 178c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index j; 179c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar fp; 180c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar parc, parl; 181c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index iter; 182c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar temp, paru; 183c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar gnorm; 184c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar dxnorm; 185c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 186c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 187c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Function Body */ 1887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Scalar dwarf = (std::numeric_limits<Scalar>::min)(); 189c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index n = qr.matrixQR().cols(); 1907faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(n==diag.size()); 1917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(n==qtb.size()); 192c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 193c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix< Scalar, Dynamic, 1 > wa1, wa2; 194c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 195c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute and store in x the gauss-newton direction. if the */ 196c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* jacobian is rank-deficient, obtain a least squares solution. */ 197c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 198c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// const Index rank = qr.nonzeroPivots(); // exactly double(0.) 199c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index rank = qr.rank(); // use a threshold 200c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = qtb; 201c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1.tail(n-rank).setZero(); 202c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank)); 203c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 204c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath x = qr.colsPermutation()*wa1; 205c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 206c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* initialize the iteration counter. */ 207c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* evaluate the function at the origin, and test */ 208c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* for acceptance of the gauss-newton direction. */ 209c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath iter = 0; 210c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = diag.cwiseProduct(x); 211c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath dxnorm = wa2.blueNorm(); 212c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fp = dxnorm - delta; 213c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fp <= Scalar(0.1) * delta) { 214c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = 0; 215c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return; 216c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 217c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 218c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* if the jacobian is not rank deficient, the newton */ 219c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* step provides a lower bound, parl, for the zero of */ 220c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* the function. otherwise set this bound to zero. */ 221c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parl = 0.; 222c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (rank==n) { 223c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm; 224c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1); 225c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = wa1.blueNorm(); 226c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parl = fp / delta / temp / temp; 227c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 228c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 229c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* calculate an upper bound, paru, for the zero of the function. */ 230c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) 231c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[j] = qr.matrixQR().col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)]; 232c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 233c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath gnorm = wa1.stableNorm(); 234c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath paru = gnorm / delta; 235c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (paru == 0.) 236c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath paru = dwarf / (std::min)(delta,Scalar(0.1)); 237c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 238c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* if the input par lies outside of the interval (parl,paru), */ 239c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* set par to the closer endpoint. */ 240c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = (std::max)(par,parl); 241c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = (std::min)(par,paru); 242c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (par == 0.) 243c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = gnorm / dxnorm; 244c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 245c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* beginning of an iteration. */ 246c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix< Scalar, Dynamic, Dynamic > s = qr.matrixQR(); 247c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath while (true) { 248c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++iter; 249c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 250c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* evaluate the function at the current value of par. */ 251c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (par == 0.) 252c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ 253c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = sqrt(par)* diag; 254c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 255c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix< Scalar, Dynamic, 1 > sdiag(n); 256c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag); 257c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 258c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = diag.cwiseProduct(x); 259c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath dxnorm = wa2.blueNorm(); 260c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = fp; 261c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fp = dxnorm - delta; 262c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 263c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* if the function is small enough, accept the current value */ 264c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* of par. also test for the exceptional cases where parl */ 265c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* is zero or the number of iterations has reached 10. */ 266c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) 267c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath break; 268c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 269c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the newton correction. */ 270c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm); 271c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // we could almost use this here, but the diagonal is outside qr, in sdiag[] 272c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1); 273c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) { 274c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[j] /= sdiag[j]; 275c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = wa1[j]; 276c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (Index i = j+1; i < n; ++i) 277c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1[i] -= s(i,j) * temp; 278c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 279c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = wa1.blueNorm(); 280c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parc = fp / delta / temp / temp; 281c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 282c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* depending on the sign of the function, update parl or paru. */ 283c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fp > 0.) 284c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parl = (std::max)(parl,par); 285c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fp < 0.) 286c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath paru = (std::min)(paru,par); 287c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 288c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute an improved estimate for par. */ 289c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = (std::max)(parl,par+parc); 290c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 291c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (iter == 0) 292c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = 0.; 293c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return; 294c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 295c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 296c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace internal 297c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 298c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace Eigen 299