13 #ifndef EIGEN_LEVENBERGMARQUARDT__H
14 #define EIGEN_LEVENBERGMARQUARDT__H
18 namespace LevenbergMarquardtSpace {
45 template<
typename FunctorType,
typename Scalar=
double>
46 class LevenbergMarquardt
133 template<
typename FunctorType,
typename Scalar>
141 m = functor.values();
144 if (
n <= 0 ||
m <
n ||
tol < 0.)
156 template<
typename FunctorType,
typename Scalar>
164 status = minimizeOneStep(
x);
169 template<
typename FunctorType,
typename Scalar>
174 m = functor.values();
176 wa1.resize(
n); wa2.resize(
n); wa3.resize(
n);
180 if (!useExternalScaling)
182 eigen_assert( (!useExternalScaling ||
diag.size()==
n) &&
"When useExternalScaling is set, the caller must provide a valid 'diag'");
193 if (useExternalScaling)
201 if ( functor(
x, fvec) < 0)
203 fnorm = fvec.stableNorm();
212 template<
typename FunctorType,
typename Scalar>
222 Index df_ret = functor.df(
x, fjac);
231 wa2 = fjac.colwise().blueNorm();
232 ColPivHouseholderQR<JacobianType> qrfac(fjac);
233 fjac = qrfac.matrixQR();
234 permutation = qrfac.colsPermutation();
239 if (!useExternalScaling)
241 diag[
j] = (wa2[
j]==0.)? 1. : wa2[
j];
245 xnorm =
diag.cwiseProduct(
x).stableNorm();
254 wa4.applyOnTheLeft(qrfac.householderQ().adjoint());
261 if (wa2[permutation.indices()[
j]] != 0.)
262 gnorm = (
std::max)(gnorm,
abs( fjac.col(
j).head(
j+1).dot(qtf.head(
j+1)/fnorm) / wa2[permutation.indices()[
j]]));
269 if (!useExternalScaling)
275 internal::lmpar2<Scalar>(qrfac,
diag, qtf,
delta, par, wa1);
280 pnorm =
diag.cwiseProduct(wa1).stableNorm();
287 if ( functor(wa2, wa4) < 0)
290 fnorm1 = wa4.stableNorm();
294 if (
Scalar(.1) * fnorm1 < fnorm)
299 wa3 = fjac.template triangularView<Upper>() * (qrfac.colsPermutation().inverse() *wa1);
302 prered = temp1 + temp2 /
Scalar(.5);
303 dirder = -(temp1 + temp2);
309 ratio = actred / prered;
316 temp =
Scalar(.5) * dirder / (dirder +
Scalar(.5) * actred);
331 wa2 =
diag.cwiseProduct(
x);
333 xnorm = wa2.stableNorm();
361 template<
typename FunctorType,
typename Scalar>
369 m = functor.values();
372 if (
n <= 0 ||
m <
n ||
tol < 0.)
380 return minimizeOptimumStorage(
x);
383 template<
typename FunctorType,
typename Scalar>
388 m = functor.values();
390 wa1.resize(
n); wa2.resize(
n); wa3.resize(
n);
399 if (!useExternalScaling)
401 eigen_assert( (!useExternalScaling ||
diag.size()==
n) &&
"When useExternalScaling is set, the caller must provide a valid 'diag'");
412 if (useExternalScaling)
420 if ( functor(
x, fvec) < 0)
422 fnorm = fvec.stableNorm();
432 template<
typename FunctorType,
typename Scalar>
451 for (
i = 0;
i <
m; ++
i) {
453 internal::rwupdt<Scalar>(fjac, wa3, qtf, fvec[
i]);
461 for (
j = 0;
j <
n; ++
j) {
464 wa2[
j] = fjac.col(
j).head(
j).stableNorm();
466 permutation.setIdentity(
n);
468 wa2 = fjac.colwise().blueNorm();
473 wa1 = fjac.diagonal();
474 fjac.diagonal() = qrfac.
hCoeffs();
477 for(
Index ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii);
479 for (
j = 0;
j <
n; ++
j) {
480 if (fjac(
j,
j) != 0.) {
482 for (
i =
j;
i <
n; ++
i)
483 sum += fjac(
i,
j) * qtf[
i];
484 temp = -sum / fjac(
j,
j);
485 for (
i =
j;
i <
n; ++
i)
486 qtf[
i] += fjac(
i,
j) * temp;
495 if (!useExternalScaling)
496 for (
j = 0;
j <
n; ++
j)
497 diag[
j] = (wa2[
j]==0.)? 1. : wa2[
j];
501 xnorm =
diag.cwiseProduct(
x).stableNorm();
510 for (
j = 0;
j <
n; ++
j)
511 if (wa2[permutation.indices()[
j]] != 0.)
512 gnorm = (
std::max)(gnorm,
abs( fjac.col(
j).head(
j+1).dot(qtf.head(
j+1)/fnorm) / wa2[permutation.indices()[
j]]));
519 if (!useExternalScaling)
525 internal::lmpar<Scalar>(fjac, permutation.indices(),
diag, qtf,
delta, par, wa1);
530 pnorm =
diag.cwiseProduct(wa1).stableNorm();
537 if ( functor(wa2, wa4) < 0)
540 fnorm1 = wa4.stableNorm();
544 if (
Scalar(.1) * fnorm1 < fnorm)
549 wa3 = fjac.topLeftCorner(
n,
n).template triangularView<Upper>() * (permutation.inverse() * wa1);
552 prered = temp1 + temp2 /
Scalar(.5);
553 dirder = -(temp1 + temp2);
559 ratio = actred / prered;
566 temp =
Scalar(.5) * dirder / (dirder +
Scalar(.5) * actred);
581 wa2 =
diag.cwiseProduct(
x);
583 xnorm = wa2.stableNorm();
611 template<
typename FunctorType,
typename Scalar>
619 status = minimizeOptimumStorageOneStep(
x);
624 template<
typename FunctorType,
typename Scalar>
627 FunctorType &functor,
634 Index m = functor.values();
637 if (
n <= 0 ||
m <
n ||
tol < 0.)
643 lm.parameters.ftol =
tol;
644 lm.parameters.xtol =
tol;
645 lm.parameters.maxfev = 200*(
n+1);
655 #endif // EIGEN_LEVENBERGMARQUARDT__H