1 // -*- coding: utf-8 2 // vim: set fileencoding=utf-8 3 4 // This file is part of Eigen, a lightweight C++ template library 5 // for linear algebra. 6 // 7 // Copyright (C) 2009 Thomas Capricelli <orzel (at) freehackers.org> 8 // 9 // This Source Code Form is subject to the terms of the Mozilla 10 // Public License v. 2.0. If a copy of the MPL was not distributed 11 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 12 13 #ifndef EIGEN_HYBRIDNONLINEARSOLVER_H 14 #define EIGEN_HYBRIDNONLINEARSOLVER_H 15 16 namespace Eigen { 17 18 namespace HybridNonLinearSolverSpace { 19 enum Status { 20 Running = -1, 21 ImproperInputParameters = 0, 22 RelativeErrorTooSmall = 1, 23 TooManyFunctionEvaluation = 2, 24 TolTooSmall = 3, 25 NotMakingProgressJacobian = 4, 26 NotMakingProgressIterations = 5, 27 UserAsked = 6 28 }; 29 } 30 31 /** 32 * \ingroup NonLinearOptimization_Module 33 * \brief Finds a zero of a system of n 34 * nonlinear functions in n variables by a modification of the Powell 35 * hybrid method ("dogleg"). 36 * 37 * The user must provide a subroutine which calculates the 38 * functions. The Jacobian is either provided by the user, or approximated 39 * using a forward-difference method. 40 * 41 */ 42 template<typename FunctorType, typename Scalar=double> 43 class HybridNonLinearSolver 44 { 45 public: 46 typedef DenseIndex Index; 47 48 HybridNonLinearSolver(FunctorType &_functor) 49 : functor(_functor) { nfev=njev=iter = 0; fnorm= 0.; useExternalScaling=false;} 50 51 struct Parameters { 52 Parameters() 53 : factor(Scalar(100.)) 54 , maxfev(1000) 55 , xtol(std::sqrt(NumTraits<Scalar>::epsilon())) 56 , nb_of_subdiagonals(-1) 57 , nb_of_superdiagonals(-1) 58 , epsfcn(Scalar(0.)) {} 59 Scalar factor; 60 Index maxfev; // maximum number of function evaluation 61 Scalar xtol; 62 Index nb_of_subdiagonals; 63 Index nb_of_superdiagonals; 64 Scalar epsfcn; 65 }; 66 typedef Matrix< Scalar, Dynamic, 1 > FVectorType; 67 typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType; 68 /* TODO: if eigen provides a triangular storage, use it here */ 69 typedef Matrix< Scalar, Dynamic, Dynamic > UpperTriangularType; 70 71 HybridNonLinearSolverSpace::Status hybrj1( 72 FVectorType &x, 73 const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) 74 ); 75 76 HybridNonLinearSolverSpace::Status solveInit(FVectorType &x); 77 HybridNonLinearSolverSpace::Status solveOneStep(FVectorType &x); 78 HybridNonLinearSolverSpace::Status solve(FVectorType &x); 79 80 HybridNonLinearSolverSpace::Status hybrd1( 81 FVectorType &x, 82 const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) 83 ); 84 85 HybridNonLinearSolverSpace::Status solveNumericalDiffInit(FVectorType &x); 86 HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(FVectorType &x); 87 HybridNonLinearSolverSpace::Status solveNumericalDiff(FVectorType &x); 88 89 void resetParameters(void) { parameters = Parameters(); } 90 Parameters parameters; 91 FVectorType fvec, qtf, diag; 92 JacobianType fjac; 93 UpperTriangularType R; 94 Index nfev; 95 Index njev; 96 Index iter; 97 Scalar fnorm; 98 bool useExternalScaling; 99 private: 100 FunctorType &functor; 101 Index n; 102 Scalar sum; 103 bool sing; 104 Scalar temp; 105 Scalar delta; 106 bool jeval; 107 Index ncsuc; 108 Scalar ratio; 109 Scalar pnorm, xnorm, fnorm1; 110 Index nslow1, nslow2; 111 Index ncfail; 112 Scalar actred, prered; 113 FVectorType wa1, wa2, wa3, wa4; 114 115 HybridNonLinearSolver& operator=(const HybridNonLinearSolver&); 116 }; 117 118 119 120 template<typename FunctorType, typename Scalar> 121 HybridNonLinearSolverSpace::Status 122 HybridNonLinearSolver<FunctorType,Scalar>::hybrj1( 123 FVectorType &x, 124 const Scalar tol 125 ) 126 { 127 n = x.size(); 128 129 /* check the input parameters for errors. */ 130 if (n <= 0 || tol < 0.) 131 return HybridNonLinearSolverSpace::ImproperInputParameters; 132 133 resetParameters(); 134 parameters.maxfev = 100*(n+1); 135 parameters.xtol = tol; 136 diag.setConstant(n, 1.); 137 useExternalScaling = true; 138 return solve(x); 139 } 140 141 template<typename FunctorType, typename Scalar> 142 HybridNonLinearSolverSpace::Status 143 HybridNonLinearSolver<FunctorType,Scalar>::solveInit(FVectorType &x) 144 { 145 n = x.size(); 146 147 wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n); 148 fvec.resize(n); 149 qtf.resize(n); 150 fjac.resize(n, n); 151 if (!useExternalScaling) 152 diag.resize(n); 153 eigen_assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); 154 155 /* Function Body */ 156 nfev = 0; 157 njev = 0; 158 159 /* check the input parameters for errors. */ 160 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0. ) 161 return HybridNonLinearSolverSpace::ImproperInputParameters; 162 if (useExternalScaling) 163 for (Index j = 0; j < n; ++j) 164 if (diag[j] <= 0.) 165 return HybridNonLinearSolverSpace::ImproperInputParameters; 166 167 /* evaluate the function at the starting point */ 168 /* and calculate its norm. */ 169 nfev = 1; 170 if ( functor(x, fvec) < 0) 171 return HybridNonLinearSolverSpace::UserAsked; 172 fnorm = fvec.stableNorm(); 173 174 /* initialize iteration counter and monitors. */ 175 iter = 1; 176 ncsuc = 0; 177 ncfail = 0; 178 nslow1 = 0; 179 nslow2 = 0; 180 181 return HybridNonLinearSolverSpace::Running; 182 } 183 184 template<typename FunctorType, typename Scalar> 185 HybridNonLinearSolverSpace::Status 186 HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(FVectorType &x) 187 { 188 using std::abs; 189 190 eigen_assert(x.size()==n); // check the caller is not cheating us 191 192 Index j; 193 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n); 194 195 jeval = true; 196 197 /* calculate the jacobian matrix. */ 198 if ( functor.df(x, fjac) < 0) 199 return HybridNonLinearSolverSpace::UserAsked; 200 ++njev; 201 202 wa2 = fjac.colwise().blueNorm(); 203 204 /* on the first iteration and if external scaling is not used, scale according */ 205 /* to the norms of the columns of the initial jacobian. */ 206 if (iter == 1) { 207 if (!useExternalScaling) 208 for (j = 0; j < n; ++j) 209 diag[j] = (wa2[j]==0.) ? 1. : wa2[j]; 210 211 /* on the first iteration, calculate the norm of the scaled x */ 212 /* and initialize the step bound delta. */ 213 xnorm = diag.cwiseProduct(x).stableNorm(); 214 delta = parameters.factor * xnorm; 215 if (delta == 0.) 216 delta = parameters.factor; 217 } 218 219 /* compute the qr factorization of the jacobian. */ 220 HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: 221 222 /* copy the triangular factor of the qr factorization into r. */ 223 R = qrfac.matrixQR(); 224 225 /* accumulate the orthogonal factor in fjac. */ 226 fjac = qrfac.householderQ(); 227 228 /* form (q transpose)*fvec and store in qtf. */ 229 qtf = fjac.transpose() * fvec; 230 231 /* rescale if necessary. */ 232 if (!useExternalScaling) 233 diag = diag.cwiseMax(wa2); 234 235 while (true) { 236 /* determine the direction p. */ 237 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1); 238 239 /* store the direction p and x + p. calculate the norm of p. */ 240 wa1 = -wa1; 241 wa2 = x + wa1; 242 pnorm = diag.cwiseProduct(wa1).stableNorm(); 243 244 /* on the first iteration, adjust the initial step bound. */ 245 if (iter == 1) 246 delta = (std::min)(delta,pnorm); 247 248 /* evaluate the function at x + p and calculate its norm. */ 249 if ( functor(wa2, wa4) < 0) 250 return HybridNonLinearSolverSpace::UserAsked; 251 ++nfev; 252 fnorm1 = wa4.stableNorm(); 253 254 /* compute the scaled actual reduction. */ 255 actred = -1.; 256 if (fnorm1 < fnorm) /* Computing 2nd power */ 257 actred = 1. - numext::abs2(fnorm1 / fnorm); 258 259 /* compute the scaled predicted reduction. */ 260 wa3 = R.template triangularView<Upper>()*wa1 + qtf; 261 temp = wa3.stableNorm(); 262 prered = 0.; 263 if (temp < fnorm) /* Computing 2nd power */ 264 prered = 1. - numext::abs2(temp / fnorm); 265 266 /* compute the ratio of the actual to the predicted reduction. */ 267 ratio = 0.; 268 if (prered > 0.) 269 ratio = actred / prered; 270 271 /* update the step bound. */ 272 if (ratio < Scalar(.1)) { 273 ncsuc = 0; 274 ++ncfail; 275 delta = Scalar(.5) * delta; 276 } else { 277 ncfail = 0; 278 ++ncsuc; 279 if (ratio >= Scalar(.5) || ncsuc > 1) 280 delta = (std::max)(delta, pnorm / Scalar(.5)); 281 if (abs(ratio - 1.) <= Scalar(.1)) { 282 delta = pnorm / Scalar(.5); 283 } 284 } 285 286 /* test for successful iteration. */ 287 if (ratio >= Scalar(1e-4)) { 288 /* successful iteration. update x, fvec, and their norms. */ 289 x = wa2; 290 wa2 = diag.cwiseProduct(x); 291 fvec = wa4; 292 xnorm = wa2.stableNorm(); 293 fnorm = fnorm1; 294 ++iter; 295 } 296 297 /* determine the progress of the iteration. */ 298 ++nslow1; 299 if (actred >= Scalar(.001)) 300 nslow1 = 0; 301 if (jeval) 302 ++nslow2; 303 if (actred >= Scalar(.1)) 304 nslow2 = 0; 305 306 /* test for convergence. */ 307 if (delta <= parameters.xtol * xnorm || fnorm == 0.) 308 return HybridNonLinearSolverSpace::RelativeErrorTooSmall; 309 310 /* tests for termination and stringent tolerances. */ 311 if (nfev >= parameters.maxfev) 312 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation; 313 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm) 314 return HybridNonLinearSolverSpace::TolTooSmall; 315 if (nslow2 == 5) 316 return HybridNonLinearSolverSpace::NotMakingProgressJacobian; 317 if (nslow1 == 10) 318 return HybridNonLinearSolverSpace::NotMakingProgressIterations; 319 320 /* criterion for recalculating jacobian. */ 321 if (ncfail == 2) 322 break; // leave inner loop and go for the next outer loop iteration 323 324 /* calculate the rank one modification to the jacobian */ 325 /* and update qtf if necessary. */ 326 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm ); 327 wa2 = fjac.transpose() * wa4; 328 if (ratio >= Scalar(1e-4)) 329 qtf = wa2; 330 wa2 = (wa2-wa3)/pnorm; 331 332 /* compute the qr factorization of the updated jacobian. */ 333 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing); 334 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens); 335 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens); 336 337 jeval = false; 338 } 339 return HybridNonLinearSolverSpace::Running; 340 } 341 342 template<typename FunctorType, typename Scalar> 343 HybridNonLinearSolverSpace::Status 344 HybridNonLinearSolver<FunctorType,Scalar>::solve(FVectorType &x) 345 { 346 HybridNonLinearSolverSpace::Status status = solveInit(x); 347 if (status==HybridNonLinearSolverSpace::ImproperInputParameters) 348 return status; 349 while (status==HybridNonLinearSolverSpace::Running) 350 status = solveOneStep(x); 351 return status; 352 } 353 354 355 356 template<typename FunctorType, typename Scalar> 357 HybridNonLinearSolverSpace::Status 358 HybridNonLinearSolver<FunctorType,Scalar>::hybrd1( 359 FVectorType &x, 360 const Scalar tol 361 ) 362 { 363 n = x.size(); 364 365 /* check the input parameters for errors. */ 366 if (n <= 0 || tol < 0.) 367 return HybridNonLinearSolverSpace::ImproperInputParameters; 368 369 resetParameters(); 370 parameters.maxfev = 200*(n+1); 371 parameters.xtol = tol; 372 373 diag.setConstant(n, 1.); 374 useExternalScaling = true; 375 return solveNumericalDiff(x); 376 } 377 378 template<typename FunctorType, typename Scalar> 379 HybridNonLinearSolverSpace::Status 380 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(FVectorType &x) 381 { 382 n = x.size(); 383 384 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1; 385 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1; 386 387 wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n); 388 qtf.resize(n); 389 fjac.resize(n, n); 390 fvec.resize(n); 391 if (!useExternalScaling) 392 diag.resize(n); 393 eigen_assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); 394 395 /* Function Body */ 396 nfev = 0; 397 njev = 0; 398 399 /* check the input parameters for errors. */ 400 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. ) 401 return HybridNonLinearSolverSpace::ImproperInputParameters; 402 if (useExternalScaling) 403 for (Index j = 0; j < n; ++j) 404 if (diag[j] <= 0.) 405 return HybridNonLinearSolverSpace::ImproperInputParameters; 406 407 /* evaluate the function at the starting point */ 408 /* and calculate its norm. */ 409 nfev = 1; 410 if ( functor(x, fvec) < 0) 411 return HybridNonLinearSolverSpace::UserAsked; 412 fnorm = fvec.stableNorm(); 413 414 /* initialize iteration counter and monitors. */ 415 iter = 1; 416 ncsuc = 0; 417 ncfail = 0; 418 nslow1 = 0; 419 nslow2 = 0; 420 421 return HybridNonLinearSolverSpace::Running; 422 } 423 424 template<typename FunctorType, typename Scalar> 425 HybridNonLinearSolverSpace::Status 426 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(FVectorType &x) 427 { 428 using std::sqrt; 429 using std::abs; 430 431 assert(x.size()==n); // check the caller is not cheating us 432 433 Index j; 434 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n); 435 436 jeval = true; 437 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1; 438 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1; 439 440 /* calculate the jacobian matrix. */ 441 if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals, parameters.epsfcn) <0) 442 return HybridNonLinearSolverSpace::UserAsked; 443 nfev += (std::min)(parameters.nb_of_subdiagonals+parameters.nb_of_superdiagonals+ 1, n); 444 445 wa2 = fjac.colwise().blueNorm(); 446 447 /* on the first iteration and if external scaling is not used, scale according */ 448 /* to the norms of the columns of the initial jacobian. */ 449 if (iter == 1) { 450 if (!useExternalScaling) 451 for (j = 0; j < n; ++j) 452 diag[j] = (wa2[j]==0.) ? 1. : wa2[j]; 453 454 /* on the first iteration, calculate the norm of the scaled x */ 455 /* and initialize the step bound delta. */ 456 xnorm = diag.cwiseProduct(x).stableNorm(); 457 delta = parameters.factor * xnorm; 458 if (delta == 0.) 459 delta = parameters.factor; 460 } 461 462 /* compute the qr factorization of the jacobian. */ 463 HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: 464 465 /* copy the triangular factor of the qr factorization into r. */ 466 R = qrfac.matrixQR(); 467 468 /* accumulate the orthogonal factor in fjac. */ 469 fjac = qrfac.householderQ(); 470 471 /* form (q transpose)*fvec and store in qtf. */ 472 qtf = fjac.transpose() * fvec; 473 474 /* rescale if necessary. */ 475 if (!useExternalScaling) 476 diag = diag.cwiseMax(wa2); 477 478 while (true) { 479 /* determine the direction p. */ 480 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1); 481 482 /* store the direction p and x + p. calculate the norm of p. */ 483 wa1 = -wa1; 484 wa2 = x + wa1; 485 pnorm = diag.cwiseProduct(wa1).stableNorm(); 486 487 /* on the first iteration, adjust the initial step bound. */ 488 if (iter == 1) 489 delta = (std::min)(delta,pnorm); 490 491 /* evaluate the function at x + p and calculate its norm. */ 492 if ( functor(wa2, wa4) < 0) 493 return HybridNonLinearSolverSpace::UserAsked; 494 ++nfev; 495 fnorm1 = wa4.stableNorm(); 496 497 /* compute the scaled actual reduction. */ 498 actred = -1.; 499 if (fnorm1 < fnorm) /* Computing 2nd power */ 500 actred = 1. - numext::abs2(fnorm1 / fnorm); 501 502 /* compute the scaled predicted reduction. */ 503 wa3 = R.template triangularView<Upper>()*wa1 + qtf; 504 temp = wa3.stableNorm(); 505 prered = 0.; 506 if (temp < fnorm) /* Computing 2nd power */ 507 prered = 1. - numext::abs2(temp / fnorm); 508 509 /* compute the ratio of the actual to the predicted reduction. */ 510 ratio = 0.; 511 if (prered > 0.) 512 ratio = actred / prered; 513 514 /* update the step bound. */ 515 if (ratio < Scalar(.1)) { 516 ncsuc = 0; 517 ++ncfail; 518 delta = Scalar(.5) * delta; 519 } else { 520 ncfail = 0; 521 ++ncsuc; 522 if (ratio >= Scalar(.5) || ncsuc > 1) 523 delta = (std::max)(delta, pnorm / Scalar(.5)); 524 if (abs(ratio - 1.) <= Scalar(.1)) { 525 delta = pnorm / Scalar(.5); 526 } 527 } 528 529 /* test for successful iteration. */ 530 if (ratio >= Scalar(1e-4)) { 531 /* successful iteration. update x, fvec, and their norms. */ 532 x = wa2; 533 wa2 = diag.cwiseProduct(x); 534 fvec = wa4; 535 xnorm = wa2.stableNorm(); 536 fnorm = fnorm1; 537 ++iter; 538 } 539 540 /* determine the progress of the iteration. */ 541 ++nslow1; 542 if (actred >= Scalar(.001)) 543 nslow1 = 0; 544 if (jeval) 545 ++nslow2; 546 if (actred >= Scalar(.1)) 547 nslow2 = 0; 548 549 /* test for convergence. */ 550 if (delta <= parameters.xtol * xnorm || fnorm == 0.) 551 return HybridNonLinearSolverSpace::RelativeErrorTooSmall; 552 553 /* tests for termination and stringent tolerances. */ 554 if (nfev >= parameters.maxfev) 555 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation; 556 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm) 557 return HybridNonLinearSolverSpace::TolTooSmall; 558 if (nslow2 == 5) 559 return HybridNonLinearSolverSpace::NotMakingProgressJacobian; 560 if (nslow1 == 10) 561 return HybridNonLinearSolverSpace::NotMakingProgressIterations; 562 563 /* criterion for recalculating jacobian. */ 564 if (ncfail == 2) 565 break; // leave inner loop and go for the next outer loop iteration 566 567 /* calculate the rank one modification to the jacobian */ 568 /* and update qtf if necessary. */ 569 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm ); 570 wa2 = fjac.transpose() * wa4; 571 if (ratio >= Scalar(1e-4)) 572 qtf = wa2; 573 wa2 = (wa2-wa3)/pnorm; 574 575 /* compute the qr factorization of the updated jacobian. */ 576 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing); 577 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens); 578 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens); 579 580 jeval = false; 581 } 582 return HybridNonLinearSolverSpace::Running; 583 } 584 585 template<typename FunctorType, typename Scalar> 586 HybridNonLinearSolverSpace::Status 587 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(FVectorType &x) 588 { 589 HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x); 590 if (status==HybridNonLinearSolverSpace::ImproperInputParameters) 591 return status; 592 while (status==HybridNonLinearSolverSpace::Running) 593 status = solveNumericalDiffOneStep(x); 594 return status; 595 } 596 597 } // end namespace Eigen 598 599 #endif // EIGEN_HYBRIDNONLINEARSOLVER_H 600 601 //vim: ai ts=4 sts=4 et sw=4 602
