lmpar.h
Go to the documentation of this file.
00001 namespace Eigen { 
00002 
00003 namespace internal {
00004 
00005 template <typename Scalar>
00006 void lmpar(
00007         Matrix< Scalar, Dynamic, Dynamic > &r,
00008         const VectorXi &ipvt,
00009         const Matrix< Scalar, Dynamic, 1 >  &diag,
00010         const Matrix< Scalar, Dynamic, 1 >  &qtb,
00011         Scalar delta,
00012         Scalar &par,
00013         Matrix< Scalar, Dynamic, 1 >  &x)
00014 {
00015     typedef DenseIndex Index;
00016 
00017     /* Local variables */
00018     Index i, j, l;
00019     Scalar fp;
00020     Scalar parc, parl;
00021     Index iter;
00022     Scalar temp, paru;
00023     Scalar gnorm;
00024     Scalar dxnorm;
00025 
00026 
00027     /* Function Body */
00028     const Scalar dwarf = std::numeric_limits<Scalar>::min();
00029     const Index n = r.cols();
00030     assert(n==diag.size());
00031     assert(n==qtb.size());
00032     assert(n==x.size());
00033 
00034     Matrix< Scalar, Dynamic, 1 >  wa1, wa2;
00035 
00036     /* compute and store in x the gauss-newton direction. if the */
00037     /* jacobian is rank-deficient, obtain a least squares solution. */
00038     Index nsing = n-1;
00039     wa1 = qtb;
00040     for (j = 0; j < n; ++j) {
00041         if (r(j,j) == 0. && nsing == n-1)
00042             nsing = j - 1;
00043         if (nsing < n-1)
00044             wa1[j] = 0.;
00045     }
00046     for (j = nsing; j>=0; --j) {
00047         wa1[j] /= r(j,j);
00048         temp = wa1[j];
00049         for (i = 0; i < j ; ++i)
00050             wa1[i] -= r(i,j) * temp;
00051     }
00052 
00053     for (j = 0; j < n; ++j)
00054         x[ipvt[j]] = wa1[j];
00055 
00056     /* initialize the iteration counter. */
00057     /* evaluate the function at the origin, and test */
00058     /* for acceptance of the gauss-newton direction. */
00059     iter = 0;
00060     wa2 = diag.cwiseProduct(x);
00061     dxnorm = wa2.blueNorm();
00062     fp = dxnorm - delta;
00063     if (fp <= Scalar(0.1) * delta) {
00064         par = 0;
00065         return;
00066     }
00067 
00068     /* if the jacobian is not rank deficient, the newton */
00069     /* step provides a lower bound, parl, for the zero of */
00070     /* the function. otherwise set this bound to zero. */
00071     parl = 0.;
00072     if (nsing >= n-1) {
00073         for (j = 0; j < n; ++j) {
00074             l = ipvt[j];
00075             wa1[j] = diag[l] * (wa2[l] / dxnorm);
00076         }
00077         // it's actually a triangularView.solveInplace(), though in a weird
00078         // way:
00079         for (j = 0; j < n; ++j) {
00080             Scalar sum = 0.;
00081             for (i = 0; i < j; ++i)
00082                 sum += r(i,j) * wa1[i];
00083             wa1[j] = (wa1[j] - sum) / r(j,j);
00084         }
00085         temp = wa1.blueNorm();
00086         parl = fp / delta / temp / temp;
00087     }
00088 
00089     /* calculate an upper bound, paru, for the zero of the function. */
00090     for (j = 0; j < n; ++j)
00091         wa1[j] = r.col(j).head(j+1).dot(qtb.head(j+1)) / diag[ipvt[j]];
00092 
00093     gnorm = wa1.stableNorm();
00094     paru = gnorm / delta;
00095     if (paru == 0.)
00096         paru = dwarf / (std::min)(delta,Scalar(0.1));
00097 
00098     /* if the input par lies outside of the interval (parl,paru), */
00099     /* set par to the closer endpoint. */
00100     par = (std::max)(par,parl);
00101     par = (std::min)(par,paru);
00102     if (par == 0.)
00103         par = gnorm / dxnorm;
00104 
00105     /* beginning of an iteration. */
00106     while (true) {
00107         ++iter;
00108 
00109         /* evaluate the function at the current value of par. */
00110         if (par == 0.)
00111             par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
00112         wa1 = sqrt(par)* diag;
00113 
00114         Matrix< Scalar, Dynamic, 1 > sdiag(n);
00115         qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag);
00116 
00117         wa2 = diag.cwiseProduct(x);
00118         dxnorm = wa2.blueNorm();
00119         temp = fp;
00120         fp = dxnorm - delta;
00121 
00122         /* if the function is small enough, accept the current value */
00123         /* of par. also test for the exceptional cases where parl */
00124         /* is zero or the number of iterations has reached 10. */
00125         if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
00126             break;
00127 
00128         /* compute the newton correction. */
00129         for (j = 0; j < n; ++j) {
00130             l = ipvt[j];
00131             wa1[j] = diag[l] * (wa2[l] / dxnorm);
00132         }
00133         for (j = 0; j < n; ++j) {
00134             wa1[j] /= sdiag[j];
00135             temp = wa1[j];
00136             for (i = j+1; i < n; ++i)
00137                 wa1[i] -= r(i,j) * temp;
00138         }
00139         temp = wa1.blueNorm();
00140         parc = fp / delta / temp / temp;
00141 
00142         /* depending on the sign of the function, update parl or paru. */
00143         if (fp > 0.)
00144             parl = (std::max)(parl,par);
00145         if (fp < 0.)
00146             paru = (std::min)(paru,par);
00147 
00148         /* compute an improved estimate for par. */
00149         /* Computing MAX */
00150         par = (std::max)(parl,par+parc);
00151 
00152         /* end of an iteration. */
00153     }
00154 
00155     /* termination. */
00156     if (iter == 0)
00157         par = 0.;
00158     return;
00159 }
00160 
00161 template <typename Scalar>
00162 void lmpar2(
00163         const ColPivHouseholderQR<Matrix< Scalar, Dynamic, Dynamic> > &qr,
00164         const Matrix< Scalar, Dynamic, 1 >  &diag,
00165         const Matrix< Scalar, Dynamic, 1 >  &qtb,
00166         Scalar delta,
00167         Scalar &par,
00168         Matrix< Scalar, Dynamic, 1 >  &x)
00169 
00170 {
00171     typedef DenseIndex Index;
00172 
00173     /* Local variables */
00174     Index j;
00175     Scalar fp;
00176     Scalar parc, parl;
00177     Index iter;
00178     Scalar temp, paru;
00179     Scalar gnorm;
00180     Scalar dxnorm;
00181 
00182 
00183     /* Function Body */
00184     const Scalar dwarf = std::numeric_limits<Scalar>::min();
00185     const Index n = qr.matrixQR().cols();
00186     assert(n==diag.size());
00187     assert(n==qtb.size());
00188 
00189     Matrix< Scalar, Dynamic, 1 >  wa1, wa2;
00190 
00191     /* compute and store in x the gauss-newton direction. if the */
00192     /* jacobian is rank-deficient, obtain a least squares solution. */
00193 
00194 //    const Index rank = qr.nonzeroPivots(); // exactly double(0.)
00195     const Index rank = qr.rank(); // use a threshold
00196     wa1 = qtb;
00197     wa1.tail(n-rank).setZero();
00198     qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));
00199 
00200     x = qr.colsPermutation()*wa1;
00201 
00202     /* initialize the iteration counter. */
00203     /* evaluate the function at the origin, and test */
00204     /* for acceptance of the gauss-newton direction. */
00205     iter = 0;
00206     wa2 = diag.cwiseProduct(x);
00207     dxnorm = wa2.blueNorm();
00208     fp = dxnorm - delta;
00209     if (fp <= Scalar(0.1) * delta) {
00210         par = 0;
00211         return;
00212     }
00213 
00214     /* if the jacobian is not rank deficient, the newton */
00215     /* step provides a lower bound, parl, for the zero of */
00216     /* the function. otherwise set this bound to zero. */
00217     parl = 0.;
00218     if (rank==n) {
00219         wa1 = qr.colsPermutation().inverse() *  diag.cwiseProduct(wa2)/dxnorm;
00220         qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
00221         temp = wa1.blueNorm();
00222         parl = fp / delta / temp / temp;
00223     }
00224 
00225     /* calculate an upper bound, paru, for the zero of the function. */
00226     for (j = 0; j < n; ++j)
00227         wa1[j] = qr.matrixQR().col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)];
00228 
00229     gnorm = wa1.stableNorm();
00230     paru = gnorm / delta;
00231     if (paru == 0.)
00232         paru = dwarf / (std::min)(delta,Scalar(0.1));
00233 
00234     /* if the input par lies outside of the interval (parl,paru), */
00235     /* set par to the closer endpoint. */
00236     par = (std::max)(par,parl);
00237     par = (std::min)(par,paru);
00238     if (par == 0.)
00239         par = gnorm / dxnorm;
00240 
00241     /* beginning of an iteration. */
00242     Matrix< Scalar, Dynamic, Dynamic > s = qr.matrixQR();
00243     while (true) {
00244         ++iter;
00245 
00246         /* evaluate the function at the current value of par. */
00247         if (par == 0.)
00248             par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
00249         wa1 = sqrt(par)* diag;
00250 
00251         Matrix< Scalar, Dynamic, 1 > sdiag(n);
00252         qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag);
00253 
00254         wa2 = diag.cwiseProduct(x);
00255         dxnorm = wa2.blueNorm();
00256         temp = fp;
00257         fp = dxnorm - delta;
00258 
00259         /* if the function is small enough, accept the current value */
00260         /* of par. also test for the exceptional cases where parl */
00261         /* is zero or the number of iterations has reached 10. */
00262         if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
00263             break;
00264 
00265         /* compute the newton correction. */
00266         wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm);
00267         // we could almost use this here, but the diagonal is outside qr, in sdiag[]
00268         // qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
00269         for (j = 0; j < n; ++j) {
00270             wa1[j] /= sdiag[j];
00271             temp = wa1[j];
00272             for (Index i = j+1; i < n; ++i)
00273                 wa1[i] -= s(i,j) * temp;
00274         }
00275         temp = wa1.blueNorm();
00276         parc = fp / delta / temp / temp;
00277 
00278         /* depending on the sign of the function, update parl or paru. */
00279         if (fp > 0.)
00280             parl = (std::max)(parl,par);
00281         if (fp < 0.)
00282             paru = (std::min)(paru,par);
00283 
00284         /* compute an improved estimate for par. */
00285         par = (std::max)(parl,par+parc);
00286     }
00287     if (iter == 0)
00288         par = 0.;
00289     return;
00290 }
00291 
00292 } // end namespace internal
00293 
00294 } // end namespace Eigen


win_eigen
Author(s): Daniel Stonier
autogenerated on Mon Oct 6 2014 12:25:11