$treeview $search $mathjax
Eigen-unsupported
3.2.5
$projectbrief
|
$projectbrief
|
$searchbox |
00001 // -*- coding: utf-8 00002 // vim: set fileencoding=utf-8 00003 00004 // This file is part of Eigen, a lightweight C++ template library 00005 // for linear algebra. 00006 // 00007 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> 00008 // 00009 // This Source Code Form is subject to the terms of the Mozilla 00010 // Public License v. 2.0. If a copy of the MPL was not distributed 00011 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00012 00013 #ifndef EIGEN_HYBRIDNONLINEARSOLVER_H 00014 #define EIGEN_HYBRIDNONLINEARSOLVER_H 00015 00016 namespace Eigen { 00017 00018 namespace HybridNonLinearSolverSpace { 00019 enum Status { 00020 Running = -1, 00021 ImproperInputParameters = 0, 00022 RelativeErrorTooSmall = 1, 00023 TooManyFunctionEvaluation = 2, 00024 TolTooSmall = 3, 00025 NotMakingProgressJacobian = 4, 00026 NotMakingProgressIterations = 5, 00027 UserAsked = 6 00028 }; 00029 } 00030 00042 template<typename FunctorType, typename Scalar=double> 00043 class HybridNonLinearSolver 00044 { 00045 public: 00046 typedef DenseIndex Index; 00047 00048 HybridNonLinearSolver(FunctorType &_functor) 00049 : functor(_functor) { nfev=njev=iter = 0; fnorm= 0.; useExternalScaling=false;} 00050 00051 struct Parameters { 00052 Parameters() 00053 : factor(Scalar(100.)) 00054 , maxfev(1000) 00055 , xtol(std::sqrt(NumTraits<Scalar>::epsilon())) 00056 , nb_of_subdiagonals(-1) 00057 , nb_of_superdiagonals(-1) 00058 , epsfcn(Scalar(0.)) {} 00059 Scalar factor; 00060 Index maxfev; // maximum number of function evaluation 00061 Scalar xtol; 00062 Index nb_of_subdiagonals; 00063 Index nb_of_superdiagonals; 00064 Scalar epsfcn; 00065 }; 00066 typedef Matrix< Scalar, Dynamic, 1 > FVectorType; 00067 typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType; 00068 /* TODO: if eigen provides a triangular storage, use it here */ 00069 typedef Matrix< Scalar, Dynamic, Dynamic > UpperTriangularType; 00070 00071 HybridNonLinearSolverSpace::Status hybrj1( 00072 FVectorType &x, 00073 const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) 00074 ); 00075 00076 HybridNonLinearSolverSpace::Status solveInit(FVectorType &x); 00077 HybridNonLinearSolverSpace::Status solveOneStep(FVectorType &x); 00078 HybridNonLinearSolverSpace::Status solve(FVectorType &x); 00079 00080 HybridNonLinearSolverSpace::Status hybrd1( 00081 FVectorType &x, 00082 const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) 00083 ); 00084 00085 HybridNonLinearSolverSpace::Status solveNumericalDiffInit(FVectorType &x); 00086 HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(FVectorType &x); 00087 HybridNonLinearSolverSpace::Status solveNumericalDiff(FVectorType &x); 00088 00089 void resetParameters(void) { parameters = Parameters(); } 00090 Parameters parameters; 00091 FVectorType fvec, qtf, diag; 00092 JacobianType fjac; 00093 UpperTriangularType R; 00094 Index nfev; 00095 Index njev; 00096 Index iter; 00097 Scalar fnorm; 00098 bool useExternalScaling; 00099 private: 00100 FunctorType &functor; 00101 Index n; 00102 Scalar sum; 00103 bool sing; 00104 Scalar temp; 00105 Scalar delta; 00106 bool jeval; 00107 Index ncsuc; 00108 Scalar ratio; 00109 Scalar pnorm, xnorm, fnorm1; 00110 Index nslow1, nslow2; 00111 Index ncfail; 00112 Scalar actred, prered; 00113 FVectorType wa1, wa2, wa3, wa4; 00114 00115 HybridNonLinearSolver& operator=(const HybridNonLinearSolver&); 00116 }; 00117 00118 00119 00120 template<typename FunctorType, typename Scalar> 00121 HybridNonLinearSolverSpace::Status 00122 HybridNonLinearSolver<FunctorType,Scalar>::hybrj1( 00123 FVectorType &x, 00124 const Scalar tol 00125 ) 00126 { 00127 n = x.size(); 00128 00129 /* check the input parameters for errors. */ 00130 if (n <= 0 || tol < 0.) 00131 return HybridNonLinearSolverSpace::ImproperInputParameters; 00132 00133 resetParameters(); 00134 parameters.maxfev = 100*(n+1); 00135 parameters.xtol = tol; 00136 diag.setConstant(n, 1.); 00137 useExternalScaling = true; 00138 return solve(x); 00139 } 00140 00141 template<typename FunctorType, typename Scalar> 00142 HybridNonLinearSolverSpace::Status 00143 HybridNonLinearSolver<FunctorType,Scalar>::solveInit(FVectorType &x) 00144 { 00145 n = x.size(); 00146 00147 wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n); 00148 fvec.resize(n); 00149 qtf.resize(n); 00150 fjac.resize(n, n); 00151 if (!useExternalScaling) 00152 diag.resize(n); 00153 eigen_assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); 00154 00155 /* Function Body */ 00156 nfev = 0; 00157 njev = 0; 00158 00159 /* check the input parameters for errors. */ 00160 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0. ) 00161 return HybridNonLinearSolverSpace::ImproperInputParameters; 00162 if (useExternalScaling) 00163 for (Index j = 0; j < n; ++j) 00164 if (diag[j] <= 0.) 00165 return HybridNonLinearSolverSpace::ImproperInputParameters; 00166 00167 /* evaluate the function at the starting point */ 00168 /* and calculate its norm. */ 00169 nfev = 1; 00170 if ( functor(x, fvec) < 0) 00171 return HybridNonLinearSolverSpace::UserAsked; 00172 fnorm = fvec.stableNorm(); 00173 00174 /* initialize iteration counter and monitors. */ 00175 iter = 1; 00176 ncsuc = 0; 00177 ncfail = 0; 00178 nslow1 = 0; 00179 nslow2 = 0; 00180 00181 return HybridNonLinearSolverSpace::Running; 00182 } 00183 00184 template<typename FunctorType, typename Scalar> 00185 HybridNonLinearSolverSpace::Status 00186 HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(FVectorType &x) 00187 { 00188 using std::abs; 00189 00190 eigen_assert(x.size()==n); // check the caller is not cheating us 00191 00192 Index j; 00193 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n); 00194 00195 jeval = true; 00196 00197 /* calculate the jacobian matrix. */ 00198 if ( functor.df(x, fjac) < 0) 00199 return HybridNonLinearSolverSpace::UserAsked; 00200 ++njev; 00201 00202 wa2 = fjac.colwise().blueNorm(); 00203 00204 /* on the first iteration and if external scaling is not used, scale according */ 00205 /* to the norms of the columns of the initial jacobian. */ 00206 if (iter == 1) { 00207 if (!useExternalScaling) 00208 for (j = 0; j < n; ++j) 00209 diag[j] = (wa2[j]==0.) ? 1. : wa2[j]; 00210 00211 /* on the first iteration, calculate the norm of the scaled x */ 00212 /* and initialize the step bound delta. */ 00213 xnorm = diag.cwiseProduct(x).stableNorm(); 00214 delta = parameters.factor * xnorm; 00215 if (delta == 0.) 00216 delta = parameters.factor; 00217 } 00218 00219 /* compute the qr factorization of the jacobian. */ 00220 HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: 00221 00222 /* copy the triangular factor of the qr factorization into r. */ 00223 R = qrfac.matrixQR(); 00224 00225 /* accumulate the orthogonal factor in fjac. */ 00226 fjac = qrfac.householderQ(); 00227 00228 /* form (q transpose)*fvec and store in qtf. */ 00229 qtf = fjac.transpose() * fvec; 00230 00231 /* rescale if necessary. */ 00232 if (!useExternalScaling) 00233 diag = diag.cwiseMax(wa2); 00234 00235 while (true) { 00236 /* determine the direction p. */ 00237 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1); 00238 00239 /* store the direction p and x + p. calculate the norm of p. */ 00240 wa1 = -wa1; 00241 wa2 = x + wa1; 00242 pnorm = diag.cwiseProduct(wa1).stableNorm(); 00243 00244 /* on the first iteration, adjust the initial step bound. */ 00245 if (iter == 1) 00246 delta = (std::min)(delta,pnorm); 00247 00248 /* evaluate the function at x + p and calculate its norm. */ 00249 if ( functor(wa2, wa4) < 0) 00250 return HybridNonLinearSolverSpace::UserAsked; 00251 ++nfev; 00252 fnorm1 = wa4.stableNorm(); 00253 00254 /* compute the scaled actual reduction. */ 00255 actred = -1.; 00256 if (fnorm1 < fnorm) /* Computing 2nd power */ 00257 actred = 1. - numext::abs2(fnorm1 / fnorm); 00258 00259 /* compute the scaled predicted reduction. */ 00260 wa3 = R.template triangularView<Upper>()*wa1 + qtf; 00261 temp = wa3.stableNorm(); 00262 prered = 0.; 00263 if (temp < fnorm) /* Computing 2nd power */ 00264 prered = 1. - numext::abs2(temp / fnorm); 00265 00266 /* compute the ratio of the actual to the predicted reduction. */ 00267 ratio = 0.; 00268 if (prered > 0.) 00269 ratio = actred / prered; 00270 00271 /* update the step bound. */ 00272 if (ratio < Scalar(.1)) { 00273 ncsuc = 0; 00274 ++ncfail; 00275 delta = Scalar(.5) * delta; 00276 } else { 00277 ncfail = 0; 00278 ++ncsuc; 00279 if (ratio >= Scalar(.5) || ncsuc > 1) 00280 delta = (std::max)(delta, pnorm / Scalar(.5)); 00281 if (abs(ratio - 1.) <= Scalar(.1)) { 00282 delta = pnorm / Scalar(.5); 00283 } 00284 } 00285 00286 /* test for successful iteration. */ 00287 if (ratio >= Scalar(1e-4)) { 00288 /* successful iteration. update x, fvec, and their norms. */ 00289 x = wa2; 00290 wa2 = diag.cwiseProduct(x); 00291 fvec = wa4; 00292 xnorm = wa2.stableNorm(); 00293 fnorm = fnorm1; 00294 ++iter; 00295 } 00296 00297 /* determine the progress of the iteration. */ 00298 ++nslow1; 00299 if (actred >= Scalar(.001)) 00300 nslow1 = 0; 00301 if (jeval) 00302 ++nslow2; 00303 if (actred >= Scalar(.1)) 00304 nslow2 = 0; 00305 00306 /* test for convergence. */ 00307 if (delta <= parameters.xtol * xnorm || fnorm == 0.) 00308 return HybridNonLinearSolverSpace::RelativeErrorTooSmall; 00309 00310 /* tests for termination and stringent tolerances. */ 00311 if (nfev >= parameters.maxfev) 00312 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation; 00313 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm) 00314 return HybridNonLinearSolverSpace::TolTooSmall; 00315 if (nslow2 == 5) 00316 return HybridNonLinearSolverSpace::NotMakingProgressJacobian; 00317 if (nslow1 == 10) 00318 return HybridNonLinearSolverSpace::NotMakingProgressIterations; 00319 00320 /* criterion for recalculating jacobian. */ 00321 if (ncfail == 2) 00322 break; // leave inner loop and go for the next outer loop iteration 00323 00324 /* calculate the rank one modification to the jacobian */ 00325 /* and update qtf if necessary. */ 00326 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm ); 00327 wa2 = fjac.transpose() * wa4; 00328 if (ratio >= Scalar(1e-4)) 00329 qtf = wa2; 00330 wa2 = (wa2-wa3)/pnorm; 00331 00332 /* compute the qr factorization of the updated jacobian. */ 00333 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing); 00334 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens); 00335 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens); 00336 00337 jeval = false; 00338 } 00339 return HybridNonLinearSolverSpace::Running; 00340 } 00341 00342 template<typename FunctorType, typename Scalar> 00343 HybridNonLinearSolverSpace::Status 00344 HybridNonLinearSolver<FunctorType,Scalar>::solve(FVectorType &x) 00345 { 00346 HybridNonLinearSolverSpace::Status status = solveInit(x); 00347 if (status==HybridNonLinearSolverSpace::ImproperInputParameters) 00348 return status; 00349 while (status==HybridNonLinearSolverSpace::Running) 00350 status = solveOneStep(x); 00351 return status; 00352 } 00353 00354 00355 00356 template<typename FunctorType, typename Scalar> 00357 HybridNonLinearSolverSpace::Status 00358 HybridNonLinearSolver<FunctorType,Scalar>::hybrd1( 00359 FVectorType &x, 00360 const Scalar tol 00361 ) 00362 { 00363 n = x.size(); 00364 00365 /* check the input parameters for errors. */ 00366 if (n <= 0 || tol < 0.) 00367 return HybridNonLinearSolverSpace::ImproperInputParameters; 00368 00369 resetParameters(); 00370 parameters.maxfev = 200*(n+1); 00371 parameters.xtol = tol; 00372 00373 diag.setConstant(n, 1.); 00374 useExternalScaling = true; 00375 return solveNumericalDiff(x); 00376 } 00377 00378 template<typename FunctorType, typename Scalar> 00379 HybridNonLinearSolverSpace::Status 00380 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(FVectorType &x) 00381 { 00382 n = x.size(); 00383 00384 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1; 00385 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1; 00386 00387 wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n); 00388 qtf.resize(n); 00389 fjac.resize(n, n); 00390 fvec.resize(n); 00391 if (!useExternalScaling) 00392 diag.resize(n); 00393 eigen_assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); 00394 00395 /* Function Body */ 00396 nfev = 0; 00397 njev = 0; 00398 00399 /* check the input parameters for errors. */ 00400 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. ) 00401 return HybridNonLinearSolverSpace::ImproperInputParameters; 00402 if (useExternalScaling) 00403 for (Index j = 0; j < n; ++j) 00404 if (diag[j] <= 0.) 00405 return HybridNonLinearSolverSpace::ImproperInputParameters; 00406 00407 /* evaluate the function at the starting point */ 00408 /* and calculate its norm. */ 00409 nfev = 1; 00410 if ( functor(x, fvec) < 0) 00411 return HybridNonLinearSolverSpace::UserAsked; 00412 fnorm = fvec.stableNorm(); 00413 00414 /* initialize iteration counter and monitors. */ 00415 iter = 1; 00416 ncsuc = 0; 00417 ncfail = 0; 00418 nslow1 = 0; 00419 nslow2 = 0; 00420 00421 return HybridNonLinearSolverSpace::Running; 00422 } 00423 00424 template<typename FunctorType, typename Scalar> 00425 HybridNonLinearSolverSpace::Status 00426 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(FVectorType &x) 00427 { 00428 using std::sqrt; 00429 using std::abs; 00430 00431 assert(x.size()==n); // check the caller is not cheating us 00432 00433 Index j; 00434 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n); 00435 00436 jeval = true; 00437 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1; 00438 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1; 00439 00440 /* calculate the jacobian matrix. */ 00441 if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals, parameters.epsfcn) <0) 00442 return HybridNonLinearSolverSpace::UserAsked; 00443 nfev += (std::min)(parameters.nb_of_subdiagonals+parameters.nb_of_superdiagonals+ 1, n); 00444 00445 wa2 = fjac.colwise().blueNorm(); 00446 00447 /* on the first iteration and if external scaling is not used, scale according */ 00448 /* to the norms of the columns of the initial jacobian. */ 00449 if (iter == 1) { 00450 if (!useExternalScaling) 00451 for (j = 0; j < n; ++j) 00452 diag[j] = (wa2[j]==0.) ? 1. : wa2[j]; 00453 00454 /* on the first iteration, calculate the norm of the scaled x */ 00455 /* and initialize the step bound delta. */ 00456 xnorm = diag.cwiseProduct(x).stableNorm(); 00457 delta = parameters.factor * xnorm; 00458 if (delta == 0.) 00459 delta = parameters.factor; 00460 } 00461 00462 /* compute the qr factorization of the jacobian. */ 00463 HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: 00464 00465 /* copy the triangular factor of the qr factorization into r. */ 00466 R = qrfac.matrixQR(); 00467 00468 /* accumulate the orthogonal factor in fjac. */ 00469 fjac = qrfac.householderQ(); 00470 00471 /* form (q transpose)*fvec and store in qtf. */ 00472 qtf = fjac.transpose() * fvec; 00473 00474 /* rescale if necessary. */ 00475 if (!useExternalScaling) 00476 diag = diag.cwiseMax(wa2); 00477 00478 while (true) { 00479 /* determine the direction p. */ 00480 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1); 00481 00482 /* store the direction p and x + p. calculate the norm of p. */ 00483 wa1 = -wa1; 00484 wa2 = x + wa1; 00485 pnorm = diag.cwiseProduct(wa1).stableNorm(); 00486 00487 /* on the first iteration, adjust the initial step bound. */ 00488 if (iter == 1) 00489 delta = (std::min)(delta,pnorm); 00490 00491 /* evaluate the function at x + p and calculate its norm. */ 00492 if ( functor(wa2, wa4) < 0) 00493 return HybridNonLinearSolverSpace::UserAsked; 00494 ++nfev; 00495 fnorm1 = wa4.stableNorm(); 00496 00497 /* compute the scaled actual reduction. */ 00498 actred = -1.; 00499 if (fnorm1 < fnorm) /* Computing 2nd power */ 00500 actred = 1. - numext::abs2(fnorm1 / fnorm); 00501 00502 /* compute the scaled predicted reduction. */ 00503 wa3 = R.template triangularView<Upper>()*wa1 + qtf; 00504 temp = wa3.stableNorm(); 00505 prered = 0.; 00506 if (temp < fnorm) /* Computing 2nd power */ 00507 prered = 1. - numext::abs2(temp / fnorm); 00508 00509 /* compute the ratio of the actual to the predicted reduction. */ 00510 ratio = 0.; 00511 if (prered > 0.) 00512 ratio = actred / prered; 00513 00514 /* update the step bound. */ 00515 if (ratio < Scalar(.1)) { 00516 ncsuc = 0; 00517 ++ncfail; 00518 delta = Scalar(.5) * delta; 00519 } else { 00520 ncfail = 0; 00521 ++ncsuc; 00522 if (ratio >= Scalar(.5) || ncsuc > 1) 00523 delta = (std::max)(delta, pnorm / Scalar(.5)); 00524 if (abs(ratio - 1.) <= Scalar(.1)) { 00525 delta = pnorm / Scalar(.5); 00526 } 00527 } 00528 00529 /* test for successful iteration. */ 00530 if (ratio >= Scalar(1e-4)) { 00531 /* successful iteration. update x, fvec, and their norms. */ 00532 x = wa2; 00533 wa2 = diag.cwiseProduct(x); 00534 fvec = wa4; 00535 xnorm = wa2.stableNorm(); 00536 fnorm = fnorm1; 00537 ++iter; 00538 } 00539 00540 /* determine the progress of the iteration. */ 00541 ++nslow1; 00542 if (actred >= Scalar(.001)) 00543 nslow1 = 0; 00544 if (jeval) 00545 ++nslow2; 00546 if (actred >= Scalar(.1)) 00547 nslow2 = 0; 00548 00549 /* test for convergence. */ 00550 if (delta <= parameters.xtol * xnorm || fnorm == 0.) 00551 return HybridNonLinearSolverSpace::RelativeErrorTooSmall; 00552 00553 /* tests for termination and stringent tolerances. */ 00554 if (nfev >= parameters.maxfev) 00555 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation; 00556 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm) 00557 return HybridNonLinearSolverSpace::TolTooSmall; 00558 if (nslow2 == 5) 00559 return HybridNonLinearSolverSpace::NotMakingProgressJacobian; 00560 if (nslow1 == 10) 00561 return HybridNonLinearSolverSpace::NotMakingProgressIterations; 00562 00563 /* criterion for recalculating jacobian. */ 00564 if (ncfail == 2) 00565 break; // leave inner loop and go for the next outer loop iteration 00566 00567 /* calculate the rank one modification to the jacobian */ 00568 /* and update qtf if necessary. */ 00569 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm ); 00570 wa2 = fjac.transpose() * wa4; 00571 if (ratio >= Scalar(1e-4)) 00572 qtf = wa2; 00573 wa2 = (wa2-wa3)/pnorm; 00574 00575 /* compute the qr factorization of the updated jacobian. */ 00576 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing); 00577 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens); 00578 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens); 00579 00580 jeval = false; 00581 } 00582 return HybridNonLinearSolverSpace::Running; 00583 } 00584 00585 template<typename FunctorType, typename Scalar> 00586 HybridNonLinearSolverSpace::Status 00587 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(FVectorType &x) 00588 { 00589 HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x); 00590 if (status==HybridNonLinearSolverSpace::ImproperInputParameters) 00591 return status; 00592 while (status==HybridNonLinearSolverSpace::Running) 00593 status = solveNumericalDiffOneStep(x); 00594 return status; 00595 } 00596 00597 } // end namespace Eigen 00598 00599 #endif // EIGEN_HYBRIDNONLINEARSOLVER_H 00600 00601 //vim: ai ts=4 sts=4 et sw=4