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) 587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez , ftol(std::sqrt(NumTraits<Scalar>::epsilon())) 597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez , xtol(std::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, 757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Scalar tol = std::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, 867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ); 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status lmstr1( 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FVectorType &x, 917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Scalar tol = std::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); 1757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_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{ 2097faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::abs; 2107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::sqrt; 2117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 2127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(x.size()==n); // check the caller is not cheating us 213c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 214c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* calculate the jacobian matrix. */ 215c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index df_ret = functor.df(x, fjac); 216c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (df_ret<0) 217c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::UserAsked; 218c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (df_ret>0) 219c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // numerical diff, we evaluated the function df_ret times 220c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath nfev += df_ret; 221c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else njev++; 222c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 223c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the qr factorization of the jacobian. */ 224c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = fjac.colwise().blueNorm(); 225c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ColPivHouseholderQR<JacobianType> qrfac(fjac); 226c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac = qrfac.matrixQR(); 227c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath permutation = qrfac.colsPermutation(); 228c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 229c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration and if external scaling is not used, scale according */ 230c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* to the norms of the columns of the initial jacobian. */ 231c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (iter == 1) { 232c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!useExternalScaling) 233c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (Index j = 0; j < n; ++j) 234c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath diag[j] = (wa2[j]==0.)? 1. : wa2[j]; 235c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 236c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration, calculate the norm of the scaled x */ 237c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* and initialize the step bound delta. */ 238c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath xnorm = diag.cwiseProduct(x).stableNorm(); 239c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = parameters.factor * xnorm; 240c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta == 0.) 241c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = parameters.factor; 242c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 243c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 244c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* form (q transpose)*fvec and store the first n components in */ 245c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* qtf. */ 246c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa4 = fvec; 247c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa4.applyOnTheLeft(qrfac.householderQ().adjoint()); 248c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qtf = wa4.head(n); 249c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 250c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the norm of the scaled gradient. */ 251c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath gnorm = 0.; 252c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fnorm != 0.) 253c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (Index j = 0; j < n; ++j) 254c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (wa2[permutation.indices()[j]] != 0.) 2557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez gnorm = (std::max)(gnorm, abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]])); 256c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 257c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* test for convergence of the gradient norm. */ 258c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (gnorm <= parameters.gtol) 259c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::CosinusTooSmall; 260c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 261c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* rescale if necessary. */ 262c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!useExternalScaling) 263c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath diag = diag.cwiseMax(wa2); 264c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 265c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath do { 266c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 267c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* determine the levenberg-marquardt parameter. */ 268c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath internal::lmpar2<Scalar>(qrfac, diag, qtf, delta, par, wa1); 269c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 270c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* store the direction p and x + p. calculate the norm of p. */ 271c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = -wa1; 272c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = x + wa1; 273c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath pnorm = diag.cwiseProduct(wa1).stableNorm(); 274c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 275c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration, adjust the initial step bound. */ 276c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (iter == 1) 277c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = (std::min)(delta,pnorm); 278c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 279c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* evaluate the function at x + p and calculate its norm. */ 280c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if ( functor(wa2, wa4) < 0) 281c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::UserAsked; 282c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++nfev; 283c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fnorm1 = wa4.stableNorm(); 284c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 285c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the scaled actual reduction. */ 286c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath actred = -1.; 287c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (Scalar(.1) * fnorm1 < fnorm) 2887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez actred = 1. - numext::abs2(fnorm1 / fnorm); 289c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 290c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the scaled predicted reduction and */ 291c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* the scaled directional derivative. */ 292c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa3 = fjac.template triangularView<Upper>() * (qrfac.colsPermutation().inverse() *wa1); 2937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp1 = numext::abs2(wa3.stableNorm() / fnorm); 2947faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp2 = numext::abs2(sqrt(par) * pnorm / fnorm); 295c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath prered = temp1 + temp2 / Scalar(.5); 296c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath dirder = -(temp1 + temp2); 297c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 298c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the ratio of the actual to the predicted */ 299c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* reduction. */ 300c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ratio = 0.; 301c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (prered != 0.) 302c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ratio = actred / prered; 303c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 304c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* update the step bound. */ 305c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (ratio <= Scalar(.25)) { 306c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (actred >= 0.) 307c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.5); 308c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (actred < 0.) 309c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred); 310c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) 311c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.1); 312c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Computing MIN */ 313c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = temp * (std::min)(delta, pnorm / Scalar(.1)); 314c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par /= temp; 315c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } else if (!(par != 0. && ratio < Scalar(.75))) { 316c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = pnorm / Scalar(.5); 317c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = Scalar(.5) * par; 318c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 319c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 320c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* test for successful iteration. */ 321c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (ratio >= Scalar(1e-4)) { 322c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* successful iteration. update x, fvec, and their norms. */ 323c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath x = wa2; 324c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = diag.cwiseProduct(x); 325c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fvec = wa4; 326c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath xnorm = wa2.stableNorm(); 327c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fnorm = fnorm1; 328c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++iter; 329c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 330c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 331c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* tests for convergence. */ 3327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm) 333c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall; 3347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.) 335c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeReductionTooSmall; 336c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta <= parameters.xtol * xnorm) 337c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeErrorTooSmall; 338c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 339c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* tests for termination and stringent tolerances. */ 340c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (nfev >= parameters.maxfev) 341c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::TooManyFunctionEvaluation; 3427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.) 343c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::FtolTooSmall; 344c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta <= NumTraits<Scalar>::epsilon() * xnorm) 345c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::XtolTooSmall; 346c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (gnorm <= NumTraits<Scalar>::epsilon()) 347c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::GtolTooSmall; 348c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 349c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } while (ratio < Scalar(1e-4)); 350c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 351c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::Running; 352c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 353c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 354c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 355c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 356c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::lmstr1( 357c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FVectorType &x, 358c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tol 359c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ) 360c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 361c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath n = x.size(); 362c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m = functor.values(); 363c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 364c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* check the input parameters for errors. */ 365c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (n <= 0 || m < n || tol < 0.) 366c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::ImproperInputParameters; 367c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 368c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath resetParameters(); 369c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parameters.ftol = tol; 370c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parameters.xtol = tol; 371c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath parameters.maxfev = 100*(n+1); 372c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 373c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return minimizeOptimumStorage(x); 374c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 375c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 376c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 377c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 378c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageInit(FVectorType &x) 379c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 380c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath n = x.size(); 381c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m = functor.values(); 382c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 383c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1.resize(n); wa2.resize(n); wa3.resize(n); 384c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa4.resize(m); 385c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fvec.resize(m); 386c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Only R is stored in fjac. Q is only used to compute 'qtf', which is 387c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Q.transpose()*rhs. qtf will be updated using givens rotation, 388c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // instead of storing them in Q. 389c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // The purpose it to only use a nxn matrix, instead of mxn here, so 390c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // that we can handle cases where m>>n : 391c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac.resize(n, n); 392c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!useExternalScaling) 393c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath diag.resize(n); 3947faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); 395c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qtf.resize(n); 396c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 397c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Function Body */ 398c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath nfev = 0; 399c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath njev = 0; 400c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 401c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* check the input parameters for errors. */ 402c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.) 403c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::ImproperInputParameters; 404c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 405c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (useExternalScaling) 406c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (Index j = 0; j < n; ++j) 407c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (diag[j] <= 0.) 408c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::ImproperInputParameters; 409c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 410c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* evaluate the function at the starting point */ 411c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* and calculate its norm. */ 412c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath nfev = 1; 413c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if ( functor(x, fvec) < 0) 414c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::UserAsked; 415c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fnorm = fvec.stableNorm(); 416c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 417c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* initialize levenberg-marquardt parameter and iteration counter. */ 418c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = 0.; 419c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath iter = 1; 420c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 421c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::NotStarted; 422c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 423c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 424c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 425c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 426c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 427c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(FVectorType &x) 428c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 4297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::abs; 4307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::sqrt; 4317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 4327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(x.size()==n); // check the caller is not cheating us 433c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 434c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index i, j; 435c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath bool sing; 436c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 437c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the qr factorization of the jacobian matrix */ 438c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* calculated one row at a time, while simultaneously */ 439c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* forming (q transpose)*fvec and storing the first */ 440c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* n components in qtf. */ 441c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qtf.fill(0.); 442c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac.fill(0.); 443c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index rownb = 2; 444c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (i = 0; i < m; ++i) { 445c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (functor.df(x, wa3, rownb) < 0) return LevenbergMarquardtSpace::UserAsked; 446c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath internal::rwupdt<Scalar>(fjac, wa3, qtf, fvec[i]); 447c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++rownb; 448c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 449c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++njev; 450c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 451c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* if the jacobian is rank deficient, call qrfac to */ 452c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* reorder its columns and update the components of qtf. */ 453c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath sing = false; 454c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) { 455c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fjac(j,j) == 0.) 456c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath sing = true; 457c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2[j] = fjac.col(j).head(j).stableNorm(); 458c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 459c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath permutation.setIdentity(n); 460c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (sing) { 461c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = fjac.colwise().blueNorm(); 462c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // TODO We have no unit test covering this code path, do not modify 463c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // until it is carefully tested 464c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ColPivHouseholderQR<JacobianType> qrfac(fjac); 465c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac = qrfac.matrixQR(); 466c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = fjac.diagonal(); 467c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac.diagonal() = qrfac.hCoeffs(); 468c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath permutation = qrfac.colsPermutation(); 469c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // TODO : avoid this: 470c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for(Index ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors 471c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 472c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) { 473c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fjac(j,j) != 0.) { 474c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath sum = 0.; 475c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (i = j; i < n; ++i) 476c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath sum += fjac(i,j) * qtf[i]; 477c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = -sum / fjac(j,j); 478c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (i = j; i < n; ++i) 479c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath qtf[i] += fjac(i,j) * temp; 480c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 481c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fjac(j,j) = wa1[j]; 482c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 483c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 484c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 485c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration and if external scaling is not used, scale according */ 486c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* to the norms of the columns of the initial jacobian. */ 487c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (iter == 1) { 488c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!useExternalScaling) 489c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) 490c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath diag[j] = (wa2[j]==0.)? 1. : wa2[j]; 491c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 492c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration, calculate the norm of the scaled x */ 493c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* and initialize the step bound delta. */ 494c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath xnorm = diag.cwiseProduct(x).stableNorm(); 495c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = parameters.factor * xnorm; 496c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta == 0.) 497c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = parameters.factor; 498c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 499c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 500c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the norm of the scaled gradient. */ 501c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath gnorm = 0.; 502c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (fnorm != 0.) 503c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (j = 0; j < n; ++j) 504c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (wa2[permutation.indices()[j]] != 0.) 5057faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez gnorm = (std::max)(gnorm, abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]])); 506c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 507c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* test for convergence of the gradient norm. */ 508c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (gnorm <= parameters.gtol) 509c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::CosinusTooSmall; 510c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 511c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* rescale if necessary. */ 512c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!useExternalScaling) 513c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath diag = diag.cwiseMax(wa2); 514c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 515c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath do { 516c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 517c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* determine the levenberg-marquardt parameter. */ 518c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath internal::lmpar<Scalar>(fjac, permutation.indices(), diag, qtf, delta, par, wa1); 519c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 520c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* store the direction p and x + p. calculate the norm of p. */ 521c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa1 = -wa1; 522c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = x + wa1; 523c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath pnorm = diag.cwiseProduct(wa1).stableNorm(); 524c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 525c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* on the first iteration, adjust the initial step bound. */ 526c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (iter == 1) 527c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = (std::min)(delta,pnorm); 528c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 529c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* evaluate the function at x + p and calculate its norm. */ 530c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if ( functor(wa2, wa4) < 0) 531c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::UserAsked; 532c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++nfev; 533c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fnorm1 = wa4.stableNorm(); 534c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 535c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the scaled actual reduction. */ 536c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath actred = -1.; 537c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (Scalar(.1) * fnorm1 < fnorm) 5387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez actred = 1. - numext::abs2(fnorm1 / fnorm); 539c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 540c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the scaled predicted reduction and */ 541c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* the scaled directional derivative. */ 542c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa3 = fjac.topLeftCorner(n,n).template triangularView<Upper>() * (permutation.inverse() * wa1); 5437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp1 = numext::abs2(wa3.stableNorm() / fnorm); 5447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez temp2 = numext::abs2(sqrt(par) * pnorm / fnorm); 545c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath prered = temp1 + temp2 / Scalar(.5); 546c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath dirder = -(temp1 + temp2); 547c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 548c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* compute the ratio of the actual to the predicted */ 549c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* reduction. */ 550c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ratio = 0.; 551c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (prered != 0.) 552c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ratio = actred / prered; 553c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 554c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* update the step bound. */ 555c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (ratio <= Scalar(.25)) { 556c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (actred >= 0.) 557c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.5); 558c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (actred < 0.) 559c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred); 560c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) 561c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath temp = Scalar(.1); 562c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Computing MIN */ 563c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = temp * (std::min)(delta, pnorm / Scalar(.1)); 564c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par /= temp; 565c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } else if (!(par != 0. && ratio < Scalar(.75))) { 566c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath delta = pnorm / Scalar(.5); 567c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath par = Scalar(.5) * par; 568c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 569c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 570c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* test for successful iteration. */ 571c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (ratio >= Scalar(1e-4)) { 572c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* successful iteration. update x, fvec, and their norms. */ 573c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath x = wa2; 574c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath wa2 = diag.cwiseProduct(x); 575c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fvec = wa4; 576c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath xnorm = wa2.stableNorm(); 577c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath fnorm = fnorm1; 578c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ++iter; 579c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 580c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 581c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* tests for convergence. */ 5827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm) 583c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall; 5847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.) 585c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeReductionTooSmall; 586c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta <= parameters.xtol * xnorm) 587c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::RelativeErrorTooSmall; 588c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 589c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* tests for termination and stringent tolerances. */ 590c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (nfev >= parameters.maxfev) 591c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::TooManyFunctionEvaluation; 5927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.) 593c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::FtolTooSmall; 594c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (delta <= NumTraits<Scalar>::epsilon() * xnorm) 595c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::XtolTooSmall; 596c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (gnorm <= NumTraits<Scalar>::epsilon()) 597c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::GtolTooSmall; 598c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 599c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } while (ratio < Scalar(1e-4)); 600c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 601c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::Running; 602c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 603c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 604c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 605c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 606c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorage(FVectorType &x) 607c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 608c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status status = minimizeOptimumStorageInit(x); 609c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (status==LevenbergMarquardtSpace::ImproperInputParameters) 610c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return status; 611c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath do { 612c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath status = minimizeOptimumStorageOneStep(x); 613c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } while (status==LevenbergMarquardtSpace::Running); 614c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return status; 615c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 616c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 617c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename FunctorType, typename Scalar> 618c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardtSpace::Status 619c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathLevenbergMarquardt<FunctorType,Scalar>::lmdif1( 620c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FunctorType &functor, 621c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath FVectorType &x, 622c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index *nfev, 623c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tol 624c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ) 625c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 626c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index n = x.size(); 627c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index m = functor.values(); 628c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 629c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* check the input parameters for errors. */ 630c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (n <= 0 || m < n || tol < 0.) 631c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return LevenbergMarquardtSpace::ImproperInputParameters; 632c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 633c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath NumericalDiff<FunctorType> numDiff(functor); 634c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // embedded LevenbergMarquardt 635c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardt<NumericalDiff<FunctorType>, Scalar > lm(numDiff); 636c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath lm.parameters.ftol = tol; 637c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath lm.parameters.xtol = tol; 638c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath lm.parameters.maxfev = 200*(n+1); 639c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 640c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x)); 641c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (nfev) 642c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * nfev = lm.nfev; 643c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return info; 644c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 645c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 646c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace Eigen 647c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 648c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif // EIGEN_LEVENBERGMARQUARDT__H 649c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 650c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath//vim: ai ts=4 sts=4 et sw=4 651