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


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:37:53