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(internal::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 = internal::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 = internal::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 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 assert(x.size()==n); // check the caller is not cheating us 189 190 Index j; 191 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n); 192 193 jeval = true; 194 195 /* calculate the jacobian matrix. */ 196 if ( functor.df(x, fjac) < 0) 197 return HybridNonLinearSolverSpace::UserAsked; 198 ++njev; 199 200 wa2 = fjac.colwise().blueNorm(); 201 202 /* on the first iteration and if external scaling is not used, scale according */ 203 /* to the norms of the columns of the initial jacobian. */ 204 if (iter == 1) { 205 if (!useExternalScaling) 206 for (j = 0; j < n; ++j) 207 diag[j] = (wa2[j]==0.) ? 1. : wa2[j]; 208 209 /* on the first iteration, calculate the norm of the scaled x */ 210 /* and initialize the step bound delta. */ 211 xnorm = diag.cwiseProduct(x).stableNorm(); 212 delta = parameters.factor * xnorm; 213 if (delta == 0.) 214 delta = parameters.factor; 215 } 216 217 /* compute the qr factorization of the jacobian. */ 218 HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: 219 220 /* copy the triangular factor of the qr factorization into r. */ 221 R = qrfac.matrixQR(); 222 223 /* accumulate the orthogonal factor in fjac. */ 224 fjac = qrfac.householderQ(); 225 226 /* form (q transpose)*fvec and store in qtf. */ 227 qtf = fjac.transpose() * fvec; 228 229 /* rescale if necessary. */ 230 if (!useExternalScaling) 231 diag = diag.cwiseMax(wa2); 232 233 while (true) { 234 /* determine the direction p. */ 235 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1); 236 237 /* store the direction p and x + p. calculate the norm of p. */ 238 wa1 = -wa1; 239 wa2 = x + wa1; 240 pnorm = diag.cwiseProduct(wa1).stableNorm(); 241 242 /* on the first iteration, adjust the initial step bound. */ 243 if (iter == 1) 244 delta = (std::min)(delta,pnorm); 245 246 /* evaluate the function at x + p and calculate its norm. */ 247 if ( functor(wa2, wa4) < 0) 248 return HybridNonLinearSolverSpace::UserAsked; 249 ++nfev; 250 fnorm1 = wa4.stableNorm(); 251 252 /* compute the scaled actual reduction. */ 253 actred = -1.; 254 if (fnorm1 < fnorm) /* Computing 2nd power */ 255 actred = 1. - internal::abs2(fnorm1 / fnorm); 256 257 /* compute the scaled predicted reduction. */ 258 wa3 = R.template triangularView<Upper>()*wa1 + qtf; 259 temp = wa3.stableNorm(); 260 prered = 0.; 261 if (temp < fnorm) /* Computing 2nd power */ 262 prered = 1. - internal::abs2(temp / fnorm); 263 264 /* compute the ratio of the actual to the predicted reduction. */ 265 ratio = 0.; 266 if (prered > 0.) 267 ratio = actred / prered; 268 269 /* update the step bound. */ 270 if (ratio < Scalar(.1)) { 271 ncsuc = 0; 272 ++ncfail; 273 delta = Scalar(.5) * delta; 274 } else { 275 ncfail = 0; 276 ++ncsuc; 277 if (ratio >= Scalar(.5) || ncsuc > 1) 278 delta = (std::max)(delta, pnorm / Scalar(.5)); 279 if (internal::abs(ratio - 1.) <= Scalar(.1)) { 280 delta = pnorm / Scalar(.5); 281 } 282 } 283 284 /* test for successful iteration. */ 285 if (ratio >= Scalar(1e-4)) { 286 /* successful iteration. update x, fvec, and their norms. */ 287 x = wa2; 288 wa2 = diag.cwiseProduct(x); 289 fvec = wa4; 290 xnorm = wa2.stableNorm(); 291 fnorm = fnorm1; 292 ++iter; 293 } 294 295 /* determine the progress of the iteration. */ 296 ++nslow1; 297 if (actred >= Scalar(.001)) 298 nslow1 = 0; 299 if (jeval) 300 ++nslow2; 301 if (actred >= Scalar(.1)) 302 nslow2 = 0; 303 304 /* test for convergence. */ 305 if (delta <= parameters.xtol * xnorm || fnorm == 0.) 306 return HybridNonLinearSolverSpace::RelativeErrorTooSmall; 307 308 /* tests for termination and stringent tolerances. */ 309 if (nfev >= parameters.maxfev) 310 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation; 311 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm) 312 return HybridNonLinearSolverSpace::TolTooSmall; 313 if (nslow2 == 5) 314 return HybridNonLinearSolverSpace::NotMakingProgressJacobian; 315 if (nslow1 == 10) 316 return HybridNonLinearSolverSpace::NotMakingProgressIterations; 317 318 /* criterion for recalculating jacobian. */ 319 if (ncfail == 2) 320 break; // leave inner loop and go for the next outer loop iteration 321 322 /* calculate the rank one modification to the jacobian */ 323 /* and update qtf if necessary. */ 324 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm ); 325 wa2 = fjac.transpose() * wa4; 326 if (ratio >= Scalar(1e-4)) 327 qtf = wa2; 328 wa2 = (wa2-wa3)/pnorm; 329 330 /* compute the qr factorization of the updated jacobian. */ 331 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing); 332 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens); 333 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens); 334 335 jeval = false; 336 } 337 return HybridNonLinearSolverSpace::Running; 338 } 339 340 template<typename FunctorType, typename Scalar> 341 HybridNonLinearSolverSpace::Status 342 HybridNonLinearSolver<FunctorType,Scalar>::solve(FVectorType &x) 343 { 344 HybridNonLinearSolverSpace::Status status = solveInit(x); 345 if (status==HybridNonLinearSolverSpace::ImproperInputParameters) 346 return status; 347 while (status==HybridNonLinearSolverSpace::Running) 348 status = solveOneStep(x); 349 return status; 350 } 351 352 353 354 template<typename FunctorType, typename Scalar> 355 HybridNonLinearSolverSpace::Status 356 HybridNonLinearSolver<FunctorType,Scalar>::hybrd1( 357 FVectorType &x, 358 const Scalar tol 359 ) 360 { 361 n = x.size(); 362 363 /* check the input parameters for errors. */ 364 if (n <= 0 || tol < 0.) 365 return HybridNonLinearSolverSpace::ImproperInputParameters; 366 367 resetParameters(); 368 parameters.maxfev = 200*(n+1); 369 parameters.xtol = tol; 370 371 diag.setConstant(n, 1.); 372 useExternalScaling = true; 373 return solveNumericalDiff(x); 374 } 375 376 template<typename FunctorType, typename Scalar> 377 HybridNonLinearSolverSpace::Status 378 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(FVectorType &x) 379 { 380 n = x.size(); 381 382 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1; 383 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1; 384 385 wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n); 386 qtf.resize(n); 387 fjac.resize(n, n); 388 fvec.resize(n); 389 if (!useExternalScaling) 390 diag.resize(n); 391 assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); 392 393 /* Function Body */ 394 nfev = 0; 395 njev = 0; 396 397 /* check the input parameters for errors. */ 398 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. ) 399 return HybridNonLinearSolverSpace::ImproperInputParameters; 400 if (useExternalScaling) 401 for (Index j = 0; j < n; ++j) 402 if (diag[j] <= 0.) 403 return HybridNonLinearSolverSpace::ImproperInputParameters; 404 405 /* evaluate the function at the starting point */ 406 /* and calculate its norm. */ 407 nfev = 1; 408 if ( functor(x, fvec) < 0) 409 return HybridNonLinearSolverSpace::UserAsked; 410 fnorm = fvec.stableNorm(); 411 412 /* initialize iteration counter and monitors. */ 413 iter = 1; 414 ncsuc = 0; 415 ncfail = 0; 416 nslow1 = 0; 417 nslow2 = 0; 418 419 return HybridNonLinearSolverSpace::Running; 420 } 421 422 template<typename FunctorType, typename Scalar> 423 HybridNonLinearSolverSpace::Status 424 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(FVectorType &x) 425 { 426 assert(x.size()==n); // check the caller is not cheating us 427 428 Index j; 429 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n); 430 431 jeval = true; 432 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1; 433 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1; 434 435 /* calculate the jacobian matrix. */ 436 if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals, parameters.epsfcn) <0) 437 return HybridNonLinearSolverSpace::UserAsked; 438 nfev += (std::min)(parameters.nb_of_subdiagonals+parameters.nb_of_superdiagonals+ 1, n); 439 440 wa2 = fjac.colwise().blueNorm(); 441 442 /* on the first iteration and if external scaling is not used, scale according */ 443 /* to the norms of the columns of the initial jacobian. */ 444 if (iter == 1) { 445 if (!useExternalScaling) 446 for (j = 0; j < n; ++j) 447 diag[j] = (wa2[j]==0.) ? 1. : wa2[j]; 448 449 /* on the first iteration, calculate the norm of the scaled x */ 450 /* and initialize the step bound delta. */ 451 xnorm = diag.cwiseProduct(x).stableNorm(); 452 delta = parameters.factor * xnorm; 453 if (delta == 0.) 454 delta = parameters.factor; 455 } 456 457 /* compute the qr factorization of the jacobian. */ 458 HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: 459 460 /* copy the triangular factor of the qr factorization into r. */ 461 R = qrfac.matrixQR(); 462 463 /* accumulate the orthogonal factor in fjac. */ 464 fjac = qrfac.householderQ(); 465 466 /* form (q transpose)*fvec and store in qtf. */ 467 qtf = fjac.transpose() * fvec; 468 469 /* rescale if necessary. */ 470 if (!useExternalScaling) 471 diag = diag.cwiseMax(wa2); 472 473 while (true) { 474 /* determine the direction p. */ 475 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1); 476 477 /* store the direction p and x + p. calculate the norm of p. */ 478 wa1 = -wa1; 479 wa2 = x + wa1; 480 pnorm = diag.cwiseProduct(wa1).stableNorm(); 481 482 /* on the first iteration, adjust the initial step bound. */ 483 if (iter == 1) 484 delta = (std::min)(delta,pnorm); 485 486 /* evaluate the function at x + p and calculate its norm. */ 487 if ( functor(wa2, wa4) < 0) 488 return HybridNonLinearSolverSpace::UserAsked; 489 ++nfev; 490 fnorm1 = wa4.stableNorm(); 491 492 /* compute the scaled actual reduction. */ 493 actred = -1.; 494 if (fnorm1 < fnorm) /* Computing 2nd power */ 495 actred = 1. - internal::abs2(fnorm1 / fnorm); 496 497 /* compute the scaled predicted reduction. */ 498 wa3 = R.template triangularView<Upper>()*wa1 + qtf; 499 temp = wa3.stableNorm(); 500 prered = 0.; 501 if (temp < fnorm) /* Computing 2nd power */ 502 prered = 1. - internal::abs2(temp / fnorm); 503 504 /* compute the ratio of the actual to the predicted reduction. */ 505 ratio = 0.; 506 if (prered > 0.) 507 ratio = actred / prered; 508 509 /* update the step bound. */ 510 if (ratio < Scalar(.1)) { 511 ncsuc = 0; 512 ++ncfail; 513 delta = Scalar(.5) * delta; 514 } else { 515 ncfail = 0; 516 ++ncsuc; 517 if (ratio >= Scalar(.5) || ncsuc > 1) 518 delta = (std::max)(delta, pnorm / Scalar(.5)); 519 if (internal::abs(ratio - 1.) <= Scalar(.1)) { 520 delta = pnorm / Scalar(.5); 521 } 522 } 523 524 /* test for successful iteration. */ 525 if (ratio >= Scalar(1e-4)) { 526 /* successful iteration. update x, fvec, and their norms. */ 527 x = wa2; 528 wa2 = diag.cwiseProduct(x); 529 fvec = wa4; 530 xnorm = wa2.stableNorm(); 531 fnorm = fnorm1; 532 ++iter; 533 } 534 535 /* determine the progress of the iteration. */ 536 ++nslow1; 537 if (actred >= Scalar(.001)) 538 nslow1 = 0; 539 if (jeval) 540 ++nslow2; 541 if (actred >= Scalar(.1)) 542 nslow2 = 0; 543 544 /* test for convergence. */ 545 if (delta <= parameters.xtol * xnorm || fnorm == 0.) 546 return HybridNonLinearSolverSpace::RelativeErrorTooSmall; 547 548 /* tests for termination and stringent tolerances. */ 549 if (nfev >= parameters.maxfev) 550 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation; 551 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm) 552 return HybridNonLinearSolverSpace::TolTooSmall; 553 if (nslow2 == 5) 554 return HybridNonLinearSolverSpace::NotMakingProgressJacobian; 555 if (nslow1 == 10) 556 return HybridNonLinearSolverSpace::NotMakingProgressIterations; 557 558 /* criterion for recalculating jacobian. */ 559 if (ncfail == 2) 560 break; // leave inner loop and go for the next outer loop iteration 561 562 /* calculate the rank one modification to the jacobian */ 563 /* and update qtf if necessary. */ 564 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm ); 565 wa2 = fjac.transpose() * wa4; 566 if (ratio >= Scalar(1e-4)) 567 qtf = wa2; 568 wa2 = (wa2-wa3)/pnorm; 569 570 /* compute the qr factorization of the updated jacobian. */ 571 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing); 572 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens); 573 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens); 574 575 jeval = false; 576 } 577 return HybridNonLinearSolverSpace::Running; 578 } 579 580 template<typename FunctorType, typename Scalar> 581 HybridNonLinearSolverSpace::Status 582 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(FVectorType &x) 583 { 584 HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x); 585 if (status==HybridNonLinearSolverSpace::ImproperInputParameters) 586 return status; 587 while (status==HybridNonLinearSolverSpace::Running) 588 status = solveNumericalDiffOneStep(x); 589 return status; 590 } 591 592 } // end namespace Eigen 593 594 #endif // EIGEN_HYBRIDNONLINEARSOLVER_H 595 596 //vim: ai ts=4 sts=4 et sw=4 597
