1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// -*- coding: utf-8 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// vim: set fileencoding=utf-8 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for linear algebra. 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This Source Code Form is subject to the terms of the Mozilla 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Public License v. 2.0. If a copy of the MPL was not distributed 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#ifndef EIGEN_LEVENBERGMARQUARDT__H 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#define EIGEN_LEVENBERGMARQUARDT__H 15c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 16c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace Eigen { 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace LevenbergMarquardtSpace { 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum Status { 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath NotStarted = -2, 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Running = -1, 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ImproperInputParameters = 0, 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath RelativeReductionTooSmall = 1, 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath RelativeErrorTooSmall = 2, 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath RelativeErrorAndReductionTooSmall = 3, 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CosinusTooSmall = 4, 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath TooManyFunctionEvaluation = 5, 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FtolTooSmall = 6, 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath XtolTooSmall = 7, 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath GtolTooSmall = 8, 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath UserAsked = 9 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \ingroup NonLinearOptimization_Module 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Performs non linear optimization over a non-linear function, 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * using a variant of the Levenberg Marquardt algorithm. 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Check wikipedia for more information. 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar=double> 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathclass LevenbergMarquardt 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathpublic: 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardt(FunctorType &_functor) 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath : functor(_functor) { nfev = njev = iter = 0; fnorm = gnorm = 0.; useExternalScaling=false; } 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef DenseIndex Index; 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath struct Parameters { 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Parameters() 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath : factor(Scalar(100.)) 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath , maxfev(400) 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath , ftol(internal::sqrt(NumTraits<Scalar>::epsilon())) 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath , xtol(internal::sqrt(NumTraits<Scalar>::epsilon())) 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath , gtol(Scalar(0.)) 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath , epsfcn(Scalar(0.)) {} 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar factor; 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index maxfev; // maximum number of function evaluation 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar ftol; 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar xtol; 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar gtol; 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar epsfcn; 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix< Scalar, Dynamic, 1 > FVectorType; 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType; 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status lmder1( 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FVectorType &x, 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon()) 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ); 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status minimize(FVectorType &x); 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x); 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x); 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath static LevenbergMarquardtSpace::Status lmdif1( 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FunctorType &functor, 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FVectorType &x, 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index *nfev, 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon()) 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ); 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status lmstr1( 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FVectorType &x, 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon()) 92c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ); 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status minimizeOptimumStorage(FVectorType &x); 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status minimizeOptimumStorageInit(FVectorType &x); 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status minimizeOptimumStorageOneStep(FVectorType &x); 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath void resetParameters(void) { parameters = Parameters(); } 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Parameters parameters; 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FVectorType fvec, qtf, diag; 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath JacobianType fjac; 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath PermutationMatrix<Dynamic,Dynamic> permutation; 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index nfev; 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index njev; 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index iter; 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar fnorm, gnorm; 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath bool useExternalScaling; 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar lm_param(void) { return par; } 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathprivate: 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FunctorType &functor; 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index n; 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index m; 115c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FVectorType wa1, wa2, wa3, wa4; 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 117c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar par, sum; 118c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar temp, temp1, temp2; 119c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar delta; 120c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar ratio; 121c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar pnorm, xnorm, fnorm1, actred, dirder, prered; 122c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardt& operator=(const LevenbergMarquardt&); 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::lmder1( 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FVectorType &x, 130c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tol 131c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ) 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath n = x.size(); 134c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m = functor.values(); 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 136c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* check the input parameters for errors. */ 137c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (n <= 0 || m < n || tol < 0.) 138c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::ImproperInputParameters; 139c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath resetParameters(); 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parameters.ftol = tol; 142c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parameters.xtol = tol; 143c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parameters.maxfev = 100*(n+1); 144c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 145c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return minimize(x); 146c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 147c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 148c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 149c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 150c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 151c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::minimize(FVectorType &x) 152c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 153c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status status = minimizeInit(x); 154c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (status==LevenbergMarquardtSpace::ImproperInputParameters) 155c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return status; 156c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath do { 157c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath status = minimizeOneStep(x); 158c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } while (status==LevenbergMarquardtSpace::Running); 159c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return status; 160c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 161c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 162c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 163c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 164c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::minimizeInit(FVectorType &x) 165c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 166c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath n = x.size(); 167c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m = functor.values(); 168c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 169c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1.resize(n); wa2.resize(n); wa3.resize(n); 170c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa4.resize(m); 171c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fvec.resize(m); 172c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac.resize(m, n); 173c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!useExternalScaling) 174c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath diag.resize(n); 175c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); 176c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qtf.resize(n); 177c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 178c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Function Body */ 179c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath nfev = 0; 180c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath njev = 0; 181c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 182c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* check the input parameters for errors. */ 183c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.) 184c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::ImproperInputParameters; 185c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 186c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (useExternalScaling) 187c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (Index j = 0; j < n; ++j) 188c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (diag[j] <= 0.) 189c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::ImproperInputParameters; 190c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 191c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* evaluate the function at the starting point */ 192c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* and calculate its norm. */ 193c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath nfev = 1; 194c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if ( functor(x, fvec) < 0) 195c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::UserAsked; 196c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fnorm = fvec.stableNorm(); 197c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 198c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* initialize levenberg-marquardt parameter and iteration counter. */ 199c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = 0.; 200c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath iter = 1; 201c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 202c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::NotStarted; 203c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 204c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 205c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 206c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 207c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(FVectorType &x) 208c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 209c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath assert(x.size()==n); // check the caller is not cheating us 210c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 211c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* calculate the jacobian matrix. */ 212c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index df_ret = functor.df(x, fjac); 213c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (df_ret<0) 214c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::UserAsked; 215c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (df_ret>0) 216c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // numerical diff, we evaluated the function df_ret times 217c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath nfev += df_ret; 218c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else njev++; 219c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 220c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the qr factorization of the jacobian. */ 221c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = fjac.colwise().blueNorm(); 222c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ColPivHouseholderQR<JacobianType> qrfac(fjac); 223c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac = qrfac.matrixQR(); 224c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath permutation = qrfac.colsPermutation(); 225c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 226c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration and if external scaling is not used, scale according */ 227c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* to the norms of the columns of the initial jacobian. */ 228c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (iter == 1) { 229c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!useExternalScaling) 230c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (Index j = 0; j < n; ++j) 231c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath diag[j] = (wa2[j]==0.)? 1. : wa2[j]; 232c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 233c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration, calculate the norm of the scaled x */ 234c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* and initialize the step bound delta. */ 235c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath xnorm = diag.cwiseProduct(x).stableNorm(); 236c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = parameters.factor * xnorm; 237c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta == 0.) 238c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = parameters.factor; 239c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 240c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 241c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* form (q transpose)*fvec and store the first n components in */ 242c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* qtf. */ 243c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa4 = fvec; 244c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa4.applyOnTheLeft(qrfac.householderQ().adjoint()); 245c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qtf = wa4.head(n); 246c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 247c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the norm of the scaled gradient. */ 248c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath gnorm = 0.; 249c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fnorm != 0.) 250c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (Index j = 0; j < n; ++j) 251c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (wa2[permutation.indices()[j]] != 0.) 252c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath gnorm = (std::max)(gnorm, internal::abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]])); 253c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 254c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* test for convergence of the gradient norm. */ 255c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (gnorm <= parameters.gtol) 256c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::CosinusTooSmall; 257c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 258c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* rescale if necessary. */ 259c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!useExternalScaling) 260c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath diag = diag.cwiseMax(wa2); 261c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 262c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath do { 263c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 264c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* determine the levenberg-marquardt parameter. */ 265c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath internal::lmpar2<Scalar>(qrfac, diag, qtf, delta, par, wa1); 266c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 267c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* store the direction p and x + p. calculate the norm of p. */ 268c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = -wa1; 269c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = x + wa1; 270c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath pnorm = diag.cwiseProduct(wa1).stableNorm(); 271c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 272c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration, adjust the initial step bound. */ 273c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (iter == 1) 274c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = (std::min)(delta,pnorm); 275c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 276c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* evaluate the function at x + p and calculate its norm. */ 277c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if ( functor(wa2, wa4) < 0) 278c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::UserAsked; 279c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++nfev; 280c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fnorm1 = wa4.stableNorm(); 281c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 282c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the scaled actual reduction. */ 283c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath actred = -1.; 284c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (Scalar(.1) * fnorm1 < fnorm) 285c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath actred = 1. - internal::abs2(fnorm1 / fnorm); 286c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 287c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the scaled predicted reduction and */ 288c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* the scaled directional derivative. */ 289c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa3 = fjac.template triangularView<Upper>() * (qrfac.colsPermutation().inverse() *wa1); 290c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp1 = internal::abs2(wa3.stableNorm() / fnorm); 291c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp2 = internal::abs2(internal::sqrt(par) * pnorm / fnorm); 292c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath prered = temp1 + temp2 / Scalar(.5); 293c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath dirder = -(temp1 + temp2); 294c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 295c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the ratio of the actual to the predicted */ 296c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* reduction. */ 297c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ratio = 0.; 298c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (prered != 0.) 299c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ratio = actred / prered; 300c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 301c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* update the step bound. */ 302c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (ratio <= Scalar(.25)) { 303c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (actred >= 0.) 304c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.5); 305c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (actred < 0.) 306c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred); 307c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) 308c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.1); 309c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Computing MIN */ 310c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = temp * (std::min)(delta, pnorm / Scalar(.1)); 311c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par /= temp; 312c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } else if (!(par != 0. && ratio < Scalar(.75))) { 313c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = pnorm / Scalar(.5); 314c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = Scalar(.5) * par; 315c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 316c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 317c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* test for successful iteration. */ 318c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (ratio >= Scalar(1e-4)) { 319c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* successful iteration. update x, fvec, and their norms. */ 320c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath x = wa2; 321c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = diag.cwiseProduct(x); 322c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fvec = wa4; 323c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath xnorm = wa2.stableNorm(); 324c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fnorm = fnorm1; 325c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++iter; 326c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 327c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 328c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* tests for convergence. */ 329c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm) 330c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall; 331c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.) 332c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeReductionTooSmall; 333c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta <= parameters.xtol * xnorm) 334c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeErrorTooSmall; 335c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 336c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* tests for termination and stringent tolerances. */ 337c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (nfev >= parameters.maxfev) 338c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::TooManyFunctionEvaluation; 339c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (internal::abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.) 340c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::FtolTooSmall; 341c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta <= NumTraits<Scalar>::epsilon() * xnorm) 342c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::XtolTooSmall; 343c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (gnorm <= NumTraits<Scalar>::epsilon()) 344c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::GtolTooSmall; 345c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 346c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } while (ratio < Scalar(1e-4)); 347c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 348c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::Running; 349c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 350c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 351c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 352c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 353c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::lmstr1( 354c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FVectorType &x, 355c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tol 356c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ) 357c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 358c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath n = x.size(); 359c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m = functor.values(); 360c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 361c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* check the input parameters for errors. */ 362c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (n <= 0 || m < n || tol < 0.) 363c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::ImproperInputParameters; 364c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 365c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath resetParameters(); 366c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parameters.ftol = tol; 367c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parameters.xtol = tol; 368c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parameters.maxfev = 100*(n+1); 369c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 370c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return minimizeOptimumStorage(x); 371c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 372c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 373c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 374c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 375c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageInit(FVectorType &x) 376c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 377c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath n = x.size(); 378c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m = functor.values(); 379c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 380c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1.resize(n); wa2.resize(n); wa3.resize(n); 381c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa4.resize(m); 382c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fvec.resize(m); 383c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Only R is stored in fjac. Q is only used to compute 'qtf', which is 384c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Q.transpose()*rhs. qtf will be updated using givens rotation, 385c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // instead of storing them in Q. 386c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // The purpose it to only use a nxn matrix, instead of mxn here, so 387c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // that we can handle cases where m>>n : 388c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac.resize(n, n); 389c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!useExternalScaling) 390c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath diag.resize(n); 391c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); 392c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qtf.resize(n); 393c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 394c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Function Body */ 395c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath nfev = 0; 396c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath njev = 0; 397c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 398c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* check the input parameters for errors. */ 399c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.) 400c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::ImproperInputParameters; 401c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 402c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (useExternalScaling) 403c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (Index j = 0; j < n; ++j) 404c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (diag[j] <= 0.) 405c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::ImproperInputParameters; 406c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 407c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* evaluate the function at the starting point */ 408c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* and calculate its norm. */ 409c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath nfev = 1; 410c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if ( functor(x, fvec) < 0) 411c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::UserAsked; 412c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fnorm = fvec.stableNorm(); 413c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 414c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* initialize levenberg-marquardt parameter and iteration counter. */ 415c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = 0.; 416c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath iter = 1; 417c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 418c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::NotStarted; 419c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 420c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 421c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 422c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 423c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 424c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(FVectorType &x) 425c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 426c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath assert(x.size()==n); // check the caller is not cheating us 427c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 428c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index i, j; 429c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath bool sing; 430c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 431c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the qr factorization of the jacobian matrix */ 432c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* calculated one row at a time, while simultaneously */ 433c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* forming (q transpose)*fvec and storing the first */ 434c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* n components in qtf. */ 435c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qtf.fill(0.); 436c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac.fill(0.); 437c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index rownb = 2; 438c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (i = 0; i < m; ++i) { 439c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (functor.df(x, wa3, rownb) < 0) return LevenbergMarquardtSpace::UserAsked; 440c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath internal::rwupdt<Scalar>(fjac, wa3, qtf, fvec[i]); 441c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++rownb; 442c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 443c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++njev; 444c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 445c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* if the jacobian is rank deficient, call qrfac to */ 446c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* reorder its columns and update the components of qtf. */ 447c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath sing = false; 448c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) { 449c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fjac(j,j) == 0.) 450c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath sing = true; 451c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2[j] = fjac.col(j).head(j).stableNorm(); 452c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 453c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath permutation.setIdentity(n); 454c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (sing) { 455c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = fjac.colwise().blueNorm(); 456c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // TODO We have no unit test covering this code path, do not modify 457c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // until it is carefully tested 458c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ColPivHouseholderQR<JacobianType> qrfac(fjac); 459c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac = qrfac.matrixQR(); 460c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = fjac.diagonal(); 461c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac.diagonal() = qrfac.hCoeffs(); 462c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath permutation = qrfac.colsPermutation(); 463c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // TODO : avoid this: 464c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for(Index ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors 465c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 466c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) { 467c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fjac(j,j) != 0.) { 468c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath sum = 0.; 469c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (i = j; i < n; ++i) 470c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath sum += fjac(i,j) * qtf[i]; 471c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = -sum / fjac(j,j); 472c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (i = j; i < n; ++i) 473c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qtf[i] += fjac(i,j) * temp; 474c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 475c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac(j,j) = wa1[j]; 476c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 477c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 478c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 479c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration and if external scaling is not used, scale according */ 480c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* to the norms of the columns of the initial jacobian. */ 481c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (iter == 1) { 482c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!useExternalScaling) 483c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) 484c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath diag[j] = (wa2[j]==0.)? 1. : wa2[j]; 485c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 486c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration, calculate the norm of the scaled x */ 487c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* and initialize the step bound delta. */ 488c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath xnorm = diag.cwiseProduct(x).stableNorm(); 489c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = parameters.factor * xnorm; 490c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta == 0.) 491c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = parameters.factor; 492c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 493c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 494c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the norm of the scaled gradient. */ 495c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath gnorm = 0.; 496c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fnorm != 0.) 497c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) 498c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (wa2[permutation.indices()[j]] != 0.) 499c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath gnorm = (std::max)(gnorm, internal::abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]])); 500c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 501c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* test for convergence of the gradient norm. */ 502c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (gnorm <= parameters.gtol) 503c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::CosinusTooSmall; 504c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 505c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* rescale if necessary. */ 506c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!useExternalScaling) 507c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath diag = diag.cwiseMax(wa2); 508c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 509c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath do { 510c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 511c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* determine the levenberg-marquardt parameter. */ 512c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath internal::lmpar<Scalar>(fjac, permutation.indices(), diag, qtf, delta, par, wa1); 513c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 514c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* store the direction p and x + p. calculate the norm of p. */ 515c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = -wa1; 516c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = x + wa1; 517c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath pnorm = diag.cwiseProduct(wa1).stableNorm(); 518c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 519c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration, adjust the initial step bound. */ 520c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (iter == 1) 521c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = (std::min)(delta,pnorm); 522c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 523c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* evaluate the function at x + p and calculate its norm. */ 524c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if ( functor(wa2, wa4) < 0) 525c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::UserAsked; 526c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++nfev; 527c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fnorm1 = wa4.stableNorm(); 528c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 529c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the scaled actual reduction. */ 530c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath actred = -1.; 531c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (Scalar(.1) * fnorm1 < fnorm) 532c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath actred = 1. - internal::abs2(fnorm1 / fnorm); 533c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 534c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the scaled predicted reduction and */ 535c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* the scaled directional derivative. */ 536c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa3 = fjac.topLeftCorner(n,n).template triangularView<Upper>() * (permutation.inverse() * wa1); 537c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp1 = internal::abs2(wa3.stableNorm() / fnorm); 538c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp2 = internal::abs2(internal::sqrt(par) * pnorm / fnorm); 539c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath prered = temp1 + temp2 / Scalar(.5); 540c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath dirder = -(temp1 + temp2); 541c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 542c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the ratio of the actual to the predicted */ 543c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* reduction. */ 544c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ratio = 0.; 545c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (prered != 0.) 546c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ratio = actred / prered; 547c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 548c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* update the step bound. */ 549c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (ratio <= Scalar(.25)) { 550c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (actred >= 0.) 551c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.5); 552c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (actred < 0.) 553c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred); 554c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) 555c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.1); 556c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Computing MIN */ 557c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = temp * (std::min)(delta, pnorm / Scalar(.1)); 558c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par /= temp; 559c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } else if (!(par != 0. && ratio < Scalar(.75))) { 560c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = pnorm / Scalar(.5); 561c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = Scalar(.5) * par; 562c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 563c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 564c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* test for successful iteration. */ 565c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (ratio >= Scalar(1e-4)) { 566c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* successful iteration. update x, fvec, and their norms. */ 567c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath x = wa2; 568c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = diag.cwiseProduct(x); 569c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fvec = wa4; 570c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath xnorm = wa2.stableNorm(); 571c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fnorm = fnorm1; 572c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++iter; 573c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 574c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 575c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* tests for convergence. */ 576c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm) 577c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall; 578c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.) 579c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeReductionTooSmall; 580c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta <= parameters.xtol * xnorm) 581c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeErrorTooSmall; 582c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 583c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* tests for termination and stringent tolerances. */ 584c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (nfev >= parameters.maxfev) 585c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::TooManyFunctionEvaluation; 586c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (internal::abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.) 587c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::FtolTooSmall; 588c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta <= NumTraits<Scalar>::epsilon() * xnorm) 589c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::XtolTooSmall; 590c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (gnorm <= NumTraits<Scalar>::epsilon()) 591c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::GtolTooSmall; 592c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 593c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } while (ratio < Scalar(1e-4)); 594c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 595c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::Running; 596c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 597c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 598c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 599c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 600c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorage(FVectorType &x) 601c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 602c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status status = minimizeOptimumStorageInit(x); 603c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (status==LevenbergMarquardtSpace::ImproperInputParameters) 604c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return status; 605c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath do { 606c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath status = minimizeOptimumStorageOneStep(x); 607c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } while (status==LevenbergMarquardtSpace::Running); 608c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return status; 609c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 610c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 611c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 612c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 613c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::lmdif1( 614c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FunctorType &functor, 615c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FVectorType &x, 616c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index *nfev, 617c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tol 618c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ) 619c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 620c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index n = x.size(); 621c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index m = functor.values(); 622c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 623c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* check the input parameters for errors. */ 624c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (n <= 0 || m < n || tol < 0.) 625c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::ImproperInputParameters; 626c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 627c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath NumericalDiff<FunctorType> numDiff(functor); 628c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // embedded LevenbergMarquardt 629c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardt<NumericalDiff<FunctorType>, Scalar > lm(numDiff); 630c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath lm.parameters.ftol = tol; 631c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath lm.parameters.xtol = tol; 632c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath lm.parameters.maxfev = 200*(n+1); 633c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 634c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x)); 635c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (nfev) 636c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * nfev = lm.nfev; 637c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return info; 638c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 639c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 640c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace Eigen 641c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 642c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif // EIGEN_LEVENBERGMARQUARDT__H 643c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 644c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath//vim: ai ts=4 sts=4 et sw=4 645