bindings-levmar (empty) → 0.1
raw patch · 47 files changed
+9258/−0 lines, 47 filesdep +basesetup-changed
Dependencies added: base
Files
- Bindings/LevMar.hsc +317/−0
- Bindings/LevMar/CurryFriendly.hs +207/−0
- LICENSE +39/−0
- Setup.hs +3/−0
- bindings-levmar.cabal +79/−0
- levmar-2.4/Axb.c +74/−0
- levmar-2.4/Axb_core.c +1040/−0
- levmar-2.4/CMakeLists.txt +52/−0
- levmar-2.4/LICENSE +340/−0
- levmar-2.4/Makefile +62/−0
- levmar-2.4/Makefile.icc +58/−0
- levmar-2.4/Makefile.vc +58/−0
- levmar-2.4/README.txt +74/−0
- levmar-2.4/compiler.h +41/−0
- levmar-2.4/expfit.c +122/−0
- levmar-2.4/levmar.vcproj +196/−0
- levmar-2.4/lm.c +83/−0
- levmar-2.4/lm.h +282/−0
- levmar-2.4/lm_core.c +847/−0
- levmar-2.4/lmbc.c +85/−0
- levmar-2.4/lmbc_core.c +949/−0
- levmar-2.4/lmblec.c +87/−0
- levmar-2.4/lmblec_core.c +413/−0
- levmar-2.4/lmdemo.c +1028/−0
- levmar-2.4/lmlec.c +80/−0
- levmar-2.4/lmlec_core.c +657/−0
- levmar-2.4/matlab/CMakeLists.txt +58/−0
- levmar-2.4/matlab/Makefile +30/−0
- levmar-2.4/matlab/Makefile.w32 +26/−0
- levmar-2.4/matlab/README.txt +35/−0
- levmar-2.4/matlab/bt3.m +11/−0
- levmar-2.4/matlab/expfit.m +8/−0
- levmar-2.4/matlab/hs01.m +6/−0
- levmar-2.4/matlab/jacbt3.m +13/−0
- levmar-2.4/matlab/jacexpfit.m +7/−0
- levmar-2.4/matlab/jachs01.m +5/−0
- levmar-2.4/matlab/jacmeyer.m +10/−0
- levmar-2.4/matlab/jacmodhs52.m +7/−0
- levmar-2.4/matlab/levmar.c +582/−0
- levmar-2.4/matlab/levmar.m +71/−0
- levmar-2.4/matlab/lmdemo.m +106/−0
- levmar-2.4/matlab/meyer.m +9/−0
- levmar-2.4/matlab/modhs52.m +7/−0
- levmar-2.4/matlab/mods235.m +5/−0
- levmar-2.4/misc.c +70/−0
- levmar-2.4/misc.h +106/−0
- levmar-2.4/misc_core.c +813/−0
+ Bindings/LevMar.hsc view
@@ -0,0 +1,317 @@+{-# LANGUAGE ForeignFunctionInterface #-}++--------------------------------------------------------------------------------+-- |+-- Module : Bindings.LevMar+-- Copyright : (c) 2009 Roel van Dijk & Bas van Dijk+-- License : BSD-style (see the file LICENSE)+--+-- Maintainer : vandijk.roel@gmail.com, v.dijk.bas@gmail.com+-- Stability : Experimental+--+-- A binding to the C levmar (Levenberg-Marquardt) library+--+-- For documentation see: <http://www.ics.forth.gr/~lourakis/levmar/>+--+--------------------------------------------------------------------------------++module Bindings.LevMar+ ( _LM_VERSION++ -- * Maximum sizes of arrays.+ , _LM_OPTS_SZ+ , _LM_INFO_SZ++ -- * Errors.+ , _LM_ERROR_LAPACK_ERROR+ , _LM_ERROR_NO_JACOBIAN+ , _LM_ERROR_NO_BOX_CONSTRAINTS+ , _LM_ERROR_FAILED_BOX_CHECK+ , _LM_ERROR_MEMORY_ALLOCATION_FAILURE+ , _LM_ERROR_CONSTRAINT_MATRIX_ROWS_GT_COLS+ , _LM_ERROR_CONSTRAINT_MATRIX_NOT_FULL_ROW_RANK+ , _LM_ERROR_TOO_FEW_MEASUREMENTS+ , _LM_ERROR_SINGULAR_MATRIX+ , _LM_ERROR_SUM_OF_SQUARES_NOT_FINITE++ -- * Default values for minimization options.+ , _LM_INIT_MU+ , _LM_STOP_THRESH+ , _LM_DIFF_DELTA++ -- * Model & Jacobian.+ , Model+ , Jacobian++ , withModel+ , withJacobian++ -- * Types of the Levenberg-Marquardt algorithms.+ , LevMarDer+ , LevMarDif+ , LevMarBCDer+ , LevMarBCDif+ , LevMarLecDer+ , LevMarLecDif+ , LevMarBLecDer+ , LevMarBLecDif++ -- * Levenberg-Marquardt algorithms.+ , dlevmar_der+ , slevmar_der+ , dlevmar_dif+ , slevmar_dif+ , dlevmar_bc_der+ , slevmar_bc_der+ , dlevmar_bc_dif+ , slevmar_bc_dif+ , dlevmar_lec_der+ , slevmar_lec_der+ , dlevmar_lec_dif+ , slevmar_lec_dif+ , dlevmar_blec_der+ , slevmar_blec_der+ , dlevmar_blec_dif+ , slevmar_blec_dif+ ) where+++import Foreign.C.Types (CInt, CFloat, CDouble)+import Foreign.Ptr (Ptr, FunPtr, freeHaskellFunPtr)+import Control.Exception (bracket)++#include <lm.h>+++-- | The version of the C levmar library.+_LM_VERSION :: String+_LM_VERSION = #const_str LM_VERSION+++--------------------------------------------------------------------------------+-- Maximum sizes of arrays.+--------------------------------------------------------------------------------++-- | The maximum size of the options array.+_LM_OPTS_SZ :: Int+_LM_OPTS_SZ = #const LM_OPTS_SZ++-- | The size of the info array.+_LM_INFO_SZ :: Int+_LM_INFO_SZ = #const LM_INFO_SZ+++--------------------------------------------------------------------------------+-- Errors.+--------------------------------------------------------------------------------++#{enum CInt,+ , _LM_ERROR_LAPACK_ERROR = LM_ERROR_LAPACK_ERROR+ , _LM_ERROR_NO_JACOBIAN = LM_ERROR_NO_JACOBIAN+ , _LM_ERROR_NO_BOX_CONSTRAINTS = LM_ERROR_NO_BOX_CONSTRAINTS+ , _LM_ERROR_FAILED_BOX_CHECK = LM_ERROR_FAILED_BOX_CHECK+ , _LM_ERROR_MEMORY_ALLOCATION_FAILURE = LM_ERROR_MEMORY_ALLOCATION_FAILURE+ , _LM_ERROR_CONSTRAINT_MATRIX_ROWS_GT_COLS = LM_ERROR_CONSTRAINT_MATRIX_ROWS_GT_COLS+ , _LM_ERROR_CONSTRAINT_MATRIX_NOT_FULL_ROW_RANK = LM_ERROR_CONSTRAINT_MATRIX_NOT_FULL_ROW_RANK+ , _LM_ERROR_TOO_FEW_MEASUREMENTS = LM_ERROR_TOO_FEW_MEASUREMENTS+ , _LM_ERROR_SINGULAR_MATRIX = LM_ERROR_SINGULAR_MATRIX+ , _LM_ERROR_SUM_OF_SQUARES_NOT_FINITE = LM_ERROR_SUM_OF_SQUARES_NOT_FINITE+ }+++--------------------------------------------------------------------------------+-- Default values for minimization options.+--------------------------------------------------------------------------------++#let const_real r = "%e", r++_LM_INIT_MU, _LM_STOP_THRESH, _LM_DIFF_DELTA :: Fractional a => a++_LM_INIT_MU = #const_real LM_INIT_MU+_LM_STOP_THRESH = #const_real LM_STOP_THRESH+_LM_DIFF_DELTA = #const_real LM_DIFF_DELTA+++--------------------------------------------------------------------------------+-- Model & Jacobian.+--------------------------------------------------------------------------------++-- | Functional relation describing measurements.+type Model r = Ptr r -- p+ -> Ptr r -- hx+ -> CInt -- m+ -> CInt -- n+ -> Ptr () -- adata+ -> IO ()++type Jacobian a = Model a++foreign import ccall "wrapper" mkModel :: Model a -> IO (FunPtr (Model a))++mkJacobian :: Jacobian a -> IO (FunPtr (Jacobian a))+mkJacobian = mkModel++withModel :: Model a -> (FunPtr (Model a) -> IO b) -> IO b+withModel m = bracket (mkModel m) freeHaskellFunPtr++withJacobian :: Jacobian a -> (FunPtr (Jacobian a) -> IO b) -> IO b+withJacobian j = bracket (mkJacobian j) freeHaskellFunPtr+++--------------------------------------------------------------------------------+-- Types of the Levenberg-Marquardt algorithms.+--------------------------------------------------------------------------------++type LevMarDer cr = FunPtr (Model cr) -- func+ -> FunPtr (Jacobian cr) -- jacf+ -> Ptr cr -- p+ -> Ptr cr -- x+ -> CInt -- m+ -> CInt -- n+ -> CInt -- itmax+ -> Ptr cr -- opts+ -> Ptr cr -- info+ -> Ptr cr -- work+ -> Ptr cr -- covar+ -> Ptr () -- adata+ -> IO CInt++type LevMarDif cr = FunPtr (Model cr) -- func+ -> Ptr cr -- p+ -> Ptr cr -- x+ -> CInt -- m+ -> CInt -- n+ -> CInt -- itmax+ -> Ptr cr -- opts+ -> Ptr cr -- info+ -> Ptr cr -- work+ -> Ptr cr -- covar+ -> Ptr () -- adata+ -> IO CInt++type LevMarBCDer cr = FunPtr (Model cr) -- func+ -> FunPtr (Jacobian cr) -- jacf+ -> Ptr cr -- p+ -> Ptr cr -- x+ -> CInt -- m+ -> CInt -- n+ -> Ptr cr -- lb+ -> Ptr cr -- ub+ -> CInt -- itmax+ -> Ptr cr -- opts+ -> Ptr cr -- info+ -> Ptr cr -- work+ -> Ptr cr -- covar+ -> Ptr () -- adata+ -> IO CInt++type LevMarBCDif cr = FunPtr (Model cr) -- func+ -> Ptr cr -- p+ -> Ptr cr -- x+ -> CInt -- m+ -> CInt -- n+ -> Ptr cr -- lb+ -> Ptr cr -- ub+ -> CInt -- itmax+ -> Ptr cr -- opts+ -> Ptr cr -- info+ -> Ptr cr -- work+ -> Ptr cr -- covar+ -> Ptr () -- adata+ -> IO CInt++type LevMarLecDer cr = FunPtr (Model cr) -- func+ -> FunPtr (Jacobian cr) -- jacf+ -> Ptr cr -- p+ -> Ptr cr -- x+ -> CInt -- m+ -> CInt -- n+ -> Ptr cr -- A+ -> Ptr cr -- B+ -> CInt -- k+ -> CInt -- itmax+ -> Ptr cr -- opts+ -> Ptr cr -- info+ -> Ptr cr -- work+ -> Ptr cr -- covar+ -> Ptr () -- adata+ -> IO CInt++type LevMarLecDif cr = FunPtr (Model cr) -- func+ -> Ptr cr -- p+ -> Ptr cr -- x+ -> CInt -- m+ -> CInt -- n+ -> Ptr cr -- A+ -> Ptr cr -- B+ -> CInt -- k+ -> CInt -- itmax+ -> Ptr cr -- opts+ -> Ptr cr -- info+ -> Ptr cr -- work+ -> Ptr cr -- covar+ -> Ptr () -- adata+ -> IO CInt++type LevMarBLecDer cr = FunPtr (Model cr) -- func+ -> FunPtr (Jacobian cr) -- jacf+ -> Ptr cr -- p+ -> Ptr cr -- x+ -> CInt -- m+ -> CInt -- n+ -> Ptr cr -- lb+ -> Ptr cr -- ub+ -> Ptr cr -- A+ -> Ptr cr -- B+ -> CInt -- k+ -> Ptr cr -- wghts+ -> CInt -- itmax+ -> Ptr cr -- opts+ -> Ptr cr -- info+ -> Ptr cr -- work+ -> Ptr cr -- covar+ -> Ptr () -- adata+ -> IO CInt++type LevMarBLecDif cr = FunPtr (Model cr) -- func+ -> Ptr cr -- p+ -> Ptr cr -- x+ -> CInt -- m+ -> CInt -- n+ -> Ptr cr -- lb+ -> Ptr cr -- ub+ -> Ptr cr -- A+ -> Ptr cr -- B+ -> CInt -- k+ -> Ptr cr -- wghts+ -> CInt -- itmax+ -> Ptr cr -- opts+ -> Ptr cr -- info+ -> Ptr cr -- work+ -> Ptr cr -- covar+ -> Ptr () -- adata+ -> IO CInt++--------------------------------------------------------------------------------+-- Levenberg-Marquardt algorithms.+--------------------------------------------------------------------------------++foreign import ccall "slevmar_der" slevmar_der :: LevMarDer CFloat+foreign import ccall "dlevmar_der" dlevmar_der :: LevMarDer CDouble+foreign import ccall "slevmar_dif" slevmar_dif :: LevMarDif CFloat+foreign import ccall "dlevmar_dif" dlevmar_dif :: LevMarDif CDouble+foreign import ccall "slevmar_bc_der" slevmar_bc_der :: LevMarBCDer CFloat+foreign import ccall "dlevmar_bc_der" dlevmar_bc_der :: LevMarBCDer CDouble+foreign import ccall "slevmar_bc_dif" slevmar_bc_dif :: LevMarBCDif CFloat+foreign import ccall "dlevmar_bc_dif" dlevmar_bc_dif :: LevMarBCDif CDouble+foreign import ccall "slevmar_lec_der" slevmar_lec_der :: LevMarLecDer CFloat+foreign import ccall "dlevmar_lec_der" dlevmar_lec_der :: LevMarLecDer CDouble+foreign import ccall "slevmar_lec_dif" slevmar_lec_dif :: LevMarLecDif CFloat+foreign import ccall "dlevmar_lec_dif" dlevmar_lec_dif :: LevMarLecDif CDouble+foreign import ccall "slevmar_blec_der" slevmar_blec_der :: LevMarBLecDer CFloat+foreign import ccall "dlevmar_blec_der" dlevmar_blec_der :: LevMarBLecDer CDouble+foreign import ccall "slevmar_blec_dif" slevmar_blec_dif :: LevMarBLecDif CFloat+foreign import ccall "dlevmar_blec_dif" dlevmar_blec_dif :: LevMarBLecDif CDouble+++-- The End ---------------------------------------------------------------------
+ Bindings/LevMar/CurryFriendly.hs view
@@ -0,0 +1,207 @@+--------------------------------------------------------------------------------+-- |+-- Module : Bindings.LevMar.CurryFriendly+-- Copyright : (c) 2009 Roel van Dijk & Bas van Dijk+-- License : BSD-style (see the file LICENSE)+--+-- Maintainer : vandijk.roel@gmail.com, v.dijk.bas@gmail.com+-- Stability : Experimental+--+-- Curry friendly variants of the Levenberg-Marquardt algorithms in 'Bindings.LevMar'.+--+-- (This module re-exports all the necessary types and function from+-- 'Bindings.LevMar', so there's no need to import that module when+-- you want to use this one.)+--+--------------------------------------------------------------------------------++module Bindings.LevMar.CurryFriendly+ ( LMA_C._LM_VERSION++ -- * Maximum sizes of arrays.+ , LMA_C._LM_OPTS_SZ+ , LMA_C._LM_INFO_SZ++ -- * Errors+ , LMA_C._LM_ERROR_LAPACK_ERROR+ , LMA_C._LM_ERROR_NO_JACOBIAN+ , LMA_C._LM_ERROR_NO_BOX_CONSTRAINTS+ , LMA_C._LM_ERROR_FAILED_BOX_CHECK+ , LMA_C._LM_ERROR_MEMORY_ALLOCATION_FAILURE+ , LMA_C._LM_ERROR_CONSTRAINT_MATRIX_ROWS_GT_COLS+ , LMA_C._LM_ERROR_CONSTRAINT_MATRIX_NOT_FULL_ROW_RANK+ , LMA_C._LM_ERROR_TOO_FEW_MEASUREMENTS+ , LMA_C._LM_ERROR_SINGULAR_MATRIX+ , LMA_C._LM_ERROR_SUM_OF_SQUARES_NOT_FINITE++ -- * Default values for options.+ , LMA_C._LM_INIT_MU+ , LMA_C._LM_STOP_THRESH+ , LMA_C._LM_DIFF_DELTA++ -- * Model & Jacobian+ , LMA_C.Model+ , LMA_C.Jacobian++ , LMA_C.withModel+ , LMA_C.withJacobian++ -- * Handy type synonyms used in the curry friendly types.+ , BoxConstraints+ , LinearConstraints+ , Weights++ -- * Curry friendly types of the Levenberg-Marquardt algorithms.+ , LevMarDer+ , LevMarDif+ , LevMarBCDer+ , LevMarBCDif+ , LevMarLecDer+ , LevMarLecDif+ , LevMarBLecDer+ , LevMarBLecDif++ -- * Curry friendly variants of the Levenberg-Marquardt algorithms in 'Bindings.Levmar'.+ , dlevmar_der+ , slevmar_der+ , dlevmar_dif+ , slevmar_dif+ , dlevmar_bc_der+ , slevmar_bc_der+ , dlevmar_bc_dif+ , slevmar_bc_dif+ , dlevmar_lec_der+ , slevmar_lec_der+ , dlevmar_lec_dif+ , slevmar_lec_dif+ , dlevmar_blec_der+ , slevmar_blec_der+ , dlevmar_blec_dif+ , slevmar_blec_dif+ ) where+++import Foreign.C.Types (CInt, CFloat, CDouble)+import Foreign.Ptr (Ptr, FunPtr)++import qualified Bindings.LevMar as LMA_C+++--------------------------------------------------------------------------------+-- Handy type synonyms used in the curry friendly types.+--------------------------------------------------------------------------------++type BoxConstraints cr a = Ptr cr -- Lower bounds+ -> Ptr cr -- Upper bounds+ -> a++type LinearConstraints cr a = Ptr cr -- Constraints matrix+ -> Ptr cr -- Right hand constraints vector+ -> CInt -- Number of constraints+ -> a++type Weights cr a = Ptr cr -- Weights+ -> a+++--------------------------------------------------------------------------------+-- Curry friendly types of the Levenberg-Marquardt algorithms.+--------------------------------------------------------------------------------++type LevMarDif cr = LMA_C.LevMarDif cr+type LevMarDer cr = FunPtr (LMA_C.Jacobian cr) -> LevMarDif cr+type LevMarBCDif cr = BoxConstraints cr (LevMarDif cr)+type LevMarBCDer cr = BoxConstraints cr (LevMarDer cr)+type LevMarLecDif cr = LinearConstraints cr (LevMarDif cr)+type LevMarLecDer cr = LinearConstraints cr (LevMarDer cr)+type LevMarBLecDif cr = BoxConstraints cr (LinearConstraints cr (Weights cr (LevMarDif cr)))+type LevMarBLecDer cr = BoxConstraints cr (LinearConstraints cr (Weights cr (LevMarDer cr)))+++--------------------------------------------------------------------------------+-- Reordering arguments to create curry friendly variants.+--------------------------------------------------------------------------------++mk_levmar_der :: LMA_C.LevMarDer cr -> LevMarDer cr+mk_levmar_der lma j f+ = lma f j++mk_levmar_bc_dif :: LMA_C.LevMarBCDif cr -> LevMarBCDif cr+mk_levmar_bc_dif lma lb ub f p x m n+ = lma f p x m n lb ub++mk_levmar_bc_der :: LMA_C.LevMarBCDer cr -> LevMarBCDer cr+mk_levmar_bc_der lma lb ub j f p x m n+ = lma f j p x m n lb ub++mk_levmar_lec_dif :: LMA_C.LevMarLecDif cr -> LevMarLecDif cr+mk_levmar_lec_dif lma a b k f p x m n+ = lma f p x m n a b k++mk_levmar_lec_der :: LMA_C.LevMarLecDer cr -> LevMarLecDer cr+mk_levmar_lec_der lma a b k j f p x m n+ = lma f j p x m n a b k++mk_levmar_blec_dif :: LMA_C.LevMarBLecDif cr -> LevMarBLecDif cr+mk_levmar_blec_dif lma lb ub a b k wghts f p x m n+ = lma f p x m n lb ub a b k wghts++mk_levmar_blec_der :: LMA_C.LevMarBLecDer cr -> LevMarBLecDer cr+mk_levmar_blec_der lma lb ub a b k wghts j f p x m n+ = lma f j p x m n lb ub a b k wghts+++--------------------------------------------------------------------------------+-- Curry friendly variants of the Levenberg-Marquardt algorithms in 'Bindings.Levmar'.+--------------------------------------------------------------------------------++slevmar_dif :: LevMarDif CFloat+slevmar_dif = LMA_C.slevmar_dif++dlevmar_dif :: LevMarDif CDouble+dlevmar_dif = LMA_C.dlevmar_dif++slevmar_der :: LevMarDer CFloat+slevmar_der = mk_levmar_der LMA_C.slevmar_der++dlevmar_der :: LevMarDer CDouble+dlevmar_der = mk_levmar_der LMA_C.dlevmar_der++slevmar_bc_dif :: LevMarBCDif CFloat+slevmar_bc_dif = mk_levmar_bc_dif LMA_C.slevmar_bc_dif++dlevmar_bc_dif :: LevMarBCDif CDouble+dlevmar_bc_dif = mk_levmar_bc_dif LMA_C.dlevmar_bc_dif++slevmar_bc_der :: LevMarBCDer CFloat+slevmar_bc_der = mk_levmar_bc_der LMA_C.slevmar_bc_der++dlevmar_bc_der :: LevMarBCDer CDouble+dlevmar_bc_der = mk_levmar_bc_der LMA_C.dlevmar_bc_der++slevmar_lec_dif :: LevMarLecDif CFloat+slevmar_lec_dif = mk_levmar_lec_dif LMA_C.slevmar_lec_dif++dlevmar_lec_dif :: LevMarLecDif CDouble+dlevmar_lec_dif = mk_levmar_lec_dif LMA_C.dlevmar_lec_dif++slevmar_lec_der :: LevMarLecDer CFloat+slevmar_lec_der = mk_levmar_lec_der LMA_C.slevmar_lec_der++dlevmar_lec_der :: LevMarLecDer CDouble+dlevmar_lec_der = mk_levmar_lec_der LMA_C.dlevmar_lec_der++slevmar_blec_dif :: LevMarBLecDif CFloat+slevmar_blec_dif = mk_levmar_blec_dif LMA_C.slevmar_blec_dif++dlevmar_blec_dif :: LevMarBLecDif CDouble+dlevmar_blec_dif = mk_levmar_blec_dif LMA_C.dlevmar_blec_dif++slevmar_blec_der :: LevMarBLecDer CFloat+slevmar_blec_der = mk_levmar_blec_der LMA_C.slevmar_blec_der++dlevmar_blec_der :: LevMarBLecDer CDouble+dlevmar_blec_der = mk_levmar_blec_der LMA_C.dlevmar_blec_der+++-- The End ---------------------------------------------------------------------
+ LICENSE view
@@ -0,0 +1,39 @@++This BSD3 license applies to all files except those in levmar-2.4.++All files in levmar-2.4 are licensed under the terms and conditions of+the GPL as detailed in levmar-2.4/LICENSE. The copyright of these+files belong to Manolis Lourakis.++Copyright (c) 2009 Roel van Dijk, Bas van Dijk++All rights reserved.++Redistribution and use in source and binary forms, with or without+modification, are permitted provided that the following conditions are+met:++ * Redistributions of source code must retain the above copyright+ notice, this list of conditions and the following disclaimer.++ * Redistributions in binary form must reproduce the above+ copyright notice, this list of conditions and the following+ disclaimer in the documentation and/or other materials provided+ with the distribution.++ * The name of Roel van Dijk and Bas van Dijk and the names of+ contributors may NOT be used to endorse or promote products+ derived from this software without specific prior written+ permission.++THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ Setup.hs view
@@ -0,0 +1,3 @@+import Distribution.Simple++main = defaultMain
+ bindings-levmar.cabal view
@@ -0,0 +1,79 @@+name: bindings-levmar+version: 0.1+cabal-version: >= 1.6+build-type: Simple+stability: experimental+author: Roel van Dijk & Bas van Dijk+maintainer: vandijk.roel@gmail.com, v.dijk.bas@gmail.com+copyright: (c) 2009 Roel van Dijk & Bas van Dijk+license: OtherLicense+license-file: LICENSE+category: numerical+synopsis: A binding to the C levmar (Levenberg-Marquardt) library+description: The Levenberg-Marquardt algorithm is an iterative+ technique that finds a local minimum of a function that+ is expressed as the sum of squares of nonlinear+ functions. It has become a standard technique for+ nonlinear least-squares problems and can be thought of+ as a combination of steepest descent and the+ Gauss-Newton method. When the current solution is far+ from the correct one, the algorithm behaves like a+ steepest descent method: slow, but guaranteed to+ converge. When the current solution is close to the+ correct solution, it becomes a Gauss-Newton method.+ .+ Both unconstrained and constrained (under linear+ equations and box constraints) Levenberg-Marquardt+ variants are included. All functions have Double and+ Float variants.+ .+ See: <http://www.ics.forth.gr/~lourakis/levmar/>+ .+ Note that the included C library is lightly patched to+ make it pure. This way the functions can be used inside+ unsafePerformIO.+ .+ A note regarding the license:+ .+ All files EXCEPT those in the levmar-2.4 directory fall+ under the BSD3 license. The levmar C library, which is+ bundled with this binding, falls under the GPL. If you+ build a program which is linked with this binding then+ it is also linked with levmar. This means such a+ program can only by distributed under the terms of the+ GPL.+++extra-source-files: levmar-2.4/LICENSE+ , levmar-2.4/*.h+ , levmar-2.4/*.c+ , levmar-2.4/*.txt+ , levmar-2.4/Makefile+ , levmar-2.4/Makefile.icc+ , levmar-2.4/Makefile.vc+ , levmar-2.4/levmar.vcproj+ , levmar-2.4/matlab/*.m+ , levmar-2.4/matlab/*.c+ , levmar-2.4/matlab/*.txt+ , levmar-2.4/matlab/Makefile+ , levmar-2.4/matlab/Makefile.w32++source-repository head+ type: darcs+ location: http://code.haskell.org/bindings-levmar++library+ build-depends: base >= 3 && < 4.2+ exposed-modules: Bindings.LevMar+ , Bindings.LevMar.CurryFriendly+ ghc-options: -Wall -O2+ cc-options: -D_OPENMP+ include-dirs: levmar-2.4+ c-sources:+ levmar-2.4/Axb.c+ levmar-2.4/lm.c+ levmar-2.4/lmbc.c+ levmar-2.4/lmblec.c+ levmar-2.4/lmlec.c+ levmar-2.4/misc.c+ pkgconfig-depends: lapack
+ levmar-2.4/Axb.c view
@@ -0,0 +1,74 @@+/////////////////////////////////////////////////////////////////////////////////+// +// Solution of linear systems involved in the Levenberg - Marquardt+// minimization algorithm+// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++/******************************************************************************** + * LAPACK-based implementations for various linear system solvers. The same core+ * code is used with appropriate #defines to derive single and double precision+ * solver versions, see also Axb_core.c+ ********************************************************************************/++#include <stdio.h>+#include <stdlib.h>+#include <math.h>++#include "lm.h"+#include "misc.h"++#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!+#endif+++#ifdef LM_DBL_PREC+/* double precision definitions */+#define LM_REAL double+#define LM_PREFIX d+#define LM_CNST(x) (x)+#ifndef HAVE_LAPACK+#include <float.h>+#define LM_REAL_EPSILON DBL_EPSILON+#endif++#include "Axb_core.c"++#undef LM_REAL+#undef LM_PREFIX+#undef LM_CNST+#undef LM_REAL_EPSILON+#endif /* LM_DBL_PREC */++#ifdef LM_SNGL_PREC+/* single precision (float) definitions */+#define LM_REAL float+#define LM_PREFIX s+#define __SUBCNST(x) x##F+#define LM_CNST(x) __SUBCNST(x) // force substitution+#ifndef HAVE_LAPACK+#define LM_REAL_EPSILON FLT_EPSILON+#endif++#include "Axb_core.c"++#undef LM_REAL+#undef LM_PREFIX+#undef __SUBCNST+#undef LM_CNST+#undef LM_REAL_EPSILON+#endif /* LM_SNGL_PREC */
+ levmar-2.4/Axb_core.c view
@@ -0,0 +1,1040 @@+/////////////////////////////////////////////////////////////////////////////////+//+// Solution of linear systems involved in the Levenberg - Marquardt+// minimization algorithm+// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////+++/* Solvers for the linear systems Ax=b. Solvers should NOT modify their A & B arguments! */+++#ifndef LM_REAL // not included by Axb.c+#error This file should not be compiled directly!+#endif+++#ifdef LINSOLVERS_RETAIN_MEMORY+#define __STATIC__ static+#else+#define __STATIC__ // empty+#endif /* LINSOLVERS_RETAIN_MEMORY */++#ifdef HAVE_LAPACK++/* prototypes of LAPACK routines */++#define GEQRF LM_MK_LAPACK_NAME(geqrf)+#define ORGQR LM_MK_LAPACK_NAME(orgqr)+#define TRTRS LM_MK_LAPACK_NAME(trtrs)+#define POTF2 LM_MK_LAPACK_NAME(potf2)+#define POTRF LM_MK_LAPACK_NAME(potrf)+#define POTRS LM_MK_LAPACK_NAME(potrs)+#define GETRF LM_MK_LAPACK_NAME(getrf)+#define GETRS LM_MK_LAPACK_NAME(getrs)+#define GESVD LM_MK_LAPACK_NAME(gesvd)+#define GESDD LM_MK_LAPACK_NAME(gesdd)++/* QR decomposition */+extern int GEQRF(int *m, int *n, LM_REAL *a, int *lda, LM_REAL *tau, LM_REAL *work, int *lwork, int *info);+extern int ORGQR(int *m, int *n, int *k, LM_REAL *a, int *lda, LM_REAL *tau, LM_REAL *work, int *lwork, int *info);++/* solution of triangular systems */+extern int TRTRS(char *uplo, char *trans, char *diag, int *n, int *nrhs, LM_REAL *a, int *lda, LM_REAL *b, int *ldb, int *info);++/* Cholesky decomposition and systems solution */+extern int POTF2(char *uplo, int *n, LM_REAL *a, int *lda, int *info);+extern int POTRF(char *uplo, int *n, LM_REAL *a, int *lda, int *info); /* block version of dpotf2 */+extern int POTRS(char *uplo, int *n, int *nrhs, LM_REAL *a, int *lda, LM_REAL *b, int *ldb, int *info);++/* LU decomposition and systems solution */+extern int GETRF(int *m, int *n, LM_REAL *a, int *lda, int *ipiv, int *info);+extern int GETRS(char *trans, int *n, int *nrhs, LM_REAL *a, int *lda, int *ipiv, LM_REAL *b, int *ldb, int *info);++/* Singular Value Decomposition (SVD) */+extern int GESVD(char *jobu, char *jobvt, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu,+ LM_REAL *vt, int *ldvt, LM_REAL *work, int *lwork, int *info);++/* lapack 3.0 new SVD routine, faster than xgesvd().+ * In case that your version of LAPACK does not include them, use the above two older routines+ */+extern int GESDD(char *jobz, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu, LM_REAL *vt, int *ldvt,+ LM_REAL *work, int *lwork, int *iwork, int *info);++/* precision-specific definitions */+#define AX_EQ_B_QR LM_ADD_PREFIX(Ax_eq_b_QR)+#define AX_EQ_B_QRLS LM_ADD_PREFIX(Ax_eq_b_QRLS)+#define AX_EQ_B_CHOL LM_ADD_PREFIX(Ax_eq_b_Chol)+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU)+#define AX_EQ_B_SVD LM_ADD_PREFIX(Ax_eq_b_SVD)++/*+ * This function returns the solution of Ax = b+ *+ * The function is based on QR decomposition with explicit computation of Q:+ * If A=Q R with Q orthogonal and R upper triangular, the linear system becomes+ * Q R x = b or R x = Q^T b.+ * The last equation can be solved directly.+ *+ * A is mxm, b is mx1+ *+ * The function returns 0 in case of error, 1 if successful+ *+ * This function is often called repetitively to solve problems of identical+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is+ * retained between calls and free'd-malloc'ed when not of the appropriate size.+ * A call with NULL as the first argument forces this memory to be released.+ */+int AX_EQ_B_QR(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)+{+__STATIC__ LM_REAL *buf=NULL;+__STATIC__ int buf_sz=0;++static int nb=0; /* no __STATIC__ decl. here! */++LM_REAL *a, *qtb, *tau, *r, *work;+int a_sz, qtb_sz, tau_sz, r_sz, tot_sz;+register int i, j;+int info, worksz, nrhs=1;+register LM_REAL sum;++ if(!A)+#ifdef LINSOLVERS_RETAIN_MEMORY+ {+ if(buf) free(buf);+ buf=NULL;+ buf_sz=0;++ return 1;+ }+#else+ return 1; /* NOP */+#endif /* LINSOLVERS_RETAIN_MEMORY */++ /* calculate required memory size */+ a_sz=m*m;+ qtb_sz=m;+ tau_sz=m;+ r_sz=m*m; /* only the upper triangular part really needed */+ if(!nb){+ LM_REAL tmp;++ worksz=-1; // workspace query; optimal size is returned in tmp+ GEQRF((int *)&m, (int *)&m, NULL, (int *)&m, NULL, (LM_REAL *)&tmp, (int *)&worksz, (int *)&info);+ nb=((int)tmp)/m; // optimal worksize is m*nb+ }+ worksz=nb*m;+ tot_sz=a_sz + qtb_sz + tau_sz + r_sz + worksz;++#ifdef LINSOLVERS_RETAIN_MEMORY+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */+ if(buf) free(buf); /* free previously allocated memory */++ buf_sz=tot_sz;+ buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_QR) "() failed!\n");+ exit(1);+ }+ }+#else+ buf_sz=tot_sz;+ buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_QR) "() failed!\n");+ exit(1);+ }+#endif /* LINSOLVERS_RETAIN_MEMORY */++ a=buf;+ qtb=a+a_sz;+ tau=qtb+qtb_sz;+ r=tau+tau_sz;+ work=r+r_sz;++ /* store A (column major!) into a */+ for(i=0; i<m; i++)+ for(j=0; j<m; j++)+ a[i+j*m]=A[i*m+j];++ /* QR decomposition of A */+ GEQRF((int *)&m, (int *)&m, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);+ /* error treatment */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQRF) " in ", AX_EQ_B_QR) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT(RCAT("Unknown LAPACK error %d for ", GEQRF) " in ", AX_EQ_B_QR) "()\n", info);+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }++ /* R is stored in the upper triangular part of a; copy it in r so that ORGQR() below won't destroy it */+ for(i=0; i<r_sz; i++)+ r[i]=a[i];++ /* compute Q using the elementary reflectors computed by the above decomposition */+ ORGQR((int *)&m, (int *)&m, (int *)&m, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", ORGQR) " in ", AX_EQ_B_QR) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT("Unknown LAPACK error (%d) in ", AX_EQ_B_QR) "()\n", info);+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }++ /* Q is now in a; compute Q^T b in qtb */+ for(i=0; i<m; i++){+ for(j=0, sum=0.0; j<m; j++)+ sum+=a[i*m+j]*B[j];+ qtb[i]=sum;+ }++ /* solve the linear system R x = Q^t b */+ TRTRS("U", "N", "N", (int *)&m, (int *)&nrhs, r, (int *)&m, qtb, (int *)&m, &info);+ /* error treatment */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QR) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QR) "()\n", info);+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }++ /* copy the result in x */+ for(i=0; i<m; i++)+ x[i]=qtb[i];++#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 1;+}++/*+ * This function returns the solution of min_x ||Ax - b||+ *+ * || . || is the second order (i.e. L2) norm. This is a least squares technique that+ * is based on QR decomposition:+ * If A=Q R with Q orthogonal and R upper triangular, the normal equations become+ * (A^T A) x = A^T b or (R^T Q^T Q R) x = A^T b or (R^T R) x = A^T b.+ * This amounts to solving R^T y = A^T b for y and then R x = y for x+ * Note that Q does not need to be explicitly computed+ *+ * A is mxn, b is mx1+ *+ * The function returns 0 in case of error, 1 if successful+ *+ * This function is often called repetitively to solve problems of identical+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is+ * retained between calls and free'd-malloc'ed when not of the appropriate size.+ * A call with NULL as the first argument forces this memory to be released.+ */+int AX_EQ_B_QRLS(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m, int n)+{+__STATIC__ LM_REAL *buf=NULL;+__STATIC__ int buf_sz=0;++static int nb=0; /* no __STATIC__ decl. here! */++LM_REAL *a, *atb, *tau, *r, *work;+int a_sz, atb_sz, tau_sz, r_sz, tot_sz;+register int i, j;+int info, worksz, nrhs=1;+register LM_REAL sum;++ if(!A)+#ifdef LINSOLVERS_RETAIN_MEMORY+ {+ if(buf) free(buf);+ buf=NULL;+ buf_sz=0;++ return 1;+ }+#else+ return 1; /* NOP */+#endif /* LINSOLVERS_RETAIN_MEMORY */++ if(m<n){+ PRINT_ERROR(RCAT("Normal equations require that the number of rows is greater than number of columns in ", AX_EQ_B_QRLS) "() [%d x %d]! -- try transposing\n", m, n);+ exit(1);+ }++ /* calculate required memory size */+ a_sz=m*n;+ atb_sz=n;+ tau_sz=n;+ r_sz=n*n;+ if(!nb){+ LM_REAL tmp;++ worksz=-1; // workspace query; optimal size is returned in tmp+ GEQRF((int *)&m, (int *)&m, NULL, (int *)&m, NULL, (LM_REAL *)&tmp, (int *)&worksz, (int *)&info);+ nb=((int)tmp)/m; // optimal worksize is m*nb+ }+ worksz=nb*m;+ tot_sz=a_sz + atb_sz + tau_sz + r_sz + worksz;++#ifdef LINSOLVERS_RETAIN_MEMORY+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */+ if(buf) free(buf); /* free previously allocated memory */++ buf_sz=tot_sz;+ buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_QRLS) "() failed!\n");+ exit(1);+ }+ }+#else+ buf_sz=tot_sz;+ buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_QRLS) "() failed!\n");+ exit(1);+ }+#endif /* LINSOLVERS_RETAIN_MEMORY */++ a=buf;+ atb=a+a_sz;+ tau=atb+atb_sz;+ r=tau+tau_sz;+ work=r+r_sz;++ /* store A (column major!) into a */+ for(i=0; i<m; i++)+ for(j=0; j<n; j++)+ a[i+j*m]=A[i*n+j];++ /* compute A^T b in atb */+ for(i=0; i<n; i++){+ for(j=0, sum=0.0; j<m; j++)+ sum+=A[j*n+i]*B[j];+ atb[i]=sum;+ }++ /* QR decomposition of A */+ GEQRF((int *)&m, (int *)&n, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);+ /* error treatment */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQRF) " in ", AX_EQ_B_QRLS) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT(RCAT("Unknown LAPACK error %d for ", GEQRF) " in ", AX_EQ_B_QRLS) "()\n", info);+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }++ /* R is stored in the upper triangular part of a. Note that a is mxn while r nxn */+ for(j=0; j<n; j++){+ for(i=0; i<=j; i++)+ r[i+j*n]=a[i+j*m];++ /* lower part is zero */+ for(i=j+1; i<n; i++)+ r[i+j*n]=0.0;+ }++ /* solve the linear system R^T y = A^t b */+ TRTRS("U", "T", "N", (int *)&n, (int *)&nrhs, r, (int *)&n, atb, (int *)&n, &info);+ /* error treatment */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QRLS) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QRLS) "()\n", info);+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }++ /* solve the linear system R x = y */+ TRTRS("U", "N", "N", (int *)&n, (int *)&nrhs, r, (int *)&n, atb, (int *)&n, &info);+ /* error treatment */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QRLS) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QRLS) "()\n", info);+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }++ /* copy the result in x */+ for(i=0; i<n; i++)+ x[i]=atb[i];++#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 1;+}++/*+ * This function returns the solution of Ax=b+ *+ * The function assumes that A is symmetric & postive definite and employs+ * the Cholesky decomposition:+ * If A=U^T U with U upper triangular, the system to be solved becomes+ * (U^T U) x = b+ * This amount to solving U^T y = b for y and then U x = y for x+ *+ * A is mxm, b is mx1+ *+ * The function returns 0 in case of error, 1 if successful+ *+ * This function is often called repetitively to solve problems of identical+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is+ * retained between calls and free'd-malloc'ed when not of the appropriate size.+ * A call with NULL as the first argument forces this memory to be released.+ */+int AX_EQ_B_CHOL(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)+{+__STATIC__ LM_REAL *buf=NULL;+__STATIC__ int buf_sz=0;++LM_REAL *a, *b;+int a_sz, b_sz, tot_sz;+register int i;+int info, nrhs=1;++ if(!A)+#ifdef LINSOLVERS_RETAIN_MEMORY+ {+ if(buf) free(buf);+ buf=NULL;+ buf_sz=0;++ return 1;+ }+#else+ return 1; /* NOP */+#endif /* LINSOLVERS_RETAIN_MEMORY */++ /* calculate required memory size */+ a_sz=m*m;+ b_sz=m;+ tot_sz=a_sz + b_sz;++#ifdef LINSOLVERS_RETAIN_MEMORY+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */+ if(buf) free(buf); /* free previously allocated memory */++ buf_sz=tot_sz;+ buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_CHOL) "() failed!\n");+ exit(1);+ }+ }+#else+ buf_sz=tot_sz;+ buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_CHOL) "() failed!\n");+ exit(1);+ }+#endif /* LINSOLVERS_RETAIN_MEMORY */++ a=buf;+ b=a+a_sz;++ /* store A into a anb B into b. A is assumed symmetric,+ * hence no transposition is needed+ */+ for(i=0; i<m; i++){+ a[i]=A[i];+ b[i]=B[i];+ }+ for(i=m; i<m*m; i++)+ a[i]=A[i];++ /* Cholesky decomposition of A */+ //POTF2("U", (int *)&m, a, (int *)&m, (int *)&info);+ POTRF("U", (int *)&m, a, (int *)&m, (int *)&info);+ /* error treatment */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", POTF2) "/", POTRF) " in ",+ AX_EQ_B_CHOL) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT(RCAT(RCAT("LAPACK error: the leading minor of order %d is not positive definite,\nthe factorization could not be completed for ", POTF2) "/", POTRF) " in ", AX_EQ_B_CHOL) "()\n", info);+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }++ /* solve using the computed Cholesky in one lapack call */+ POTRS("U", (int *)&m, (int *)&nrhs, a, (int *)&m, b, (int *)&m, &info);+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", POTRS) " in ", AX_EQ_B_CHOL) "()\n", -info);+ exit(1);+ }++#if 0+ /* alternative: solve the linear system U^T y = b ... */+ TRTRS("U", "T", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, b, (int *)&m, &info);+ /* error treatment */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_CHOL) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_CHOL) "()\n", info);+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }++ /* ... solve the linear system U x = y */+ TRTRS("U", "N", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, b, (int *)&m, &info);+ /* error treatment */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) "in ", AX_EQ_B_CHOL) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_CHOL) "()\n", info);+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }+#endif /* 0 */++ /* copy the result in x */+ for(i=0; i<m; i++)+ x[i]=b[i];++#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 1;+}++/*+ * This function returns the solution of Ax = b+ *+ * The function employs LU decomposition:+ * If A=L U with L lower and U upper triangular, then the original system+ * amounts to solving+ * L y = b, U x = y+ *+ * A is mxm, b is mx1+ *+ * The function returns 0 in case of error, 1 if successful+ *+ * This function is often called repetitively to solve problems of identical+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is+ * retained between calls and free'd-malloc'ed when not of the appropriate size.+ * A call with NULL as the first argument forces this memory to be released.+ */+int AX_EQ_B_LU(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)+{+__STATIC__ LM_REAL *buf=NULL;+__STATIC__ int buf_sz=0;++int a_sz, ipiv_sz, b_sz, tot_sz;+register int i, j;+int info, *ipiv, nrhs=1;+LM_REAL *a, *b;++ if(!A)+#ifdef LINSOLVERS_RETAIN_MEMORY+ {+ if(buf) free(buf);+ buf=NULL;+ buf_sz=0;++ return 1;+ }+#else+ return 1; /* NOP */+#endif /* LINSOLVERS_RETAIN_MEMORY */++ /* calculate required memory size */+ ipiv_sz=m;+ a_sz=m*m;+ b_sz=m;+ tot_sz=(a_sz + b_sz)*sizeof(LM_REAL) + ipiv_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */++#ifdef LINSOLVERS_RETAIN_MEMORY+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */+ if(buf) free(buf); /* free previously allocated memory */++ buf_sz=tot_sz;+ buf=(LM_REAL *)malloc(buf_sz);+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");+ exit(1);+ }+ }+#else+ buf_sz=tot_sz;+ buf=(LM_REAL *)malloc(buf_sz);+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");+ exit(1);+ }+#endif /* LINSOLVERS_RETAIN_MEMORY */++ a=buf;+ b=a+a_sz;+ ipiv=(int *)(b+b_sz);++ /* store A (column major!) into a and B into b */+ for(i=0; i<m; i++){+ for(j=0; j<m; j++)+ a[i+j*m]=A[i*m+j];++ b[i]=B[i];+ }++ /* LU decomposition for A */+ GETRF((int *)&m, (int *)&m, a, (int *)&m, ipiv, (int *)&info);+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("argument %d of ", GETRF) " illegal in ", AX_EQ_B_LU) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT(RCAT("singular matrix A for ", GETRF) " in ", AX_EQ_B_LU) "()\n");+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }++ /* solve the system with the computed LU */+ GETRS("N", (int *)&m, (int *)&nrhs, a, (int *)&m, ipiv, b, (int *)&m, (int *)&info);+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("argument %d of ", GETRS) " illegal in ", AX_EQ_B_LU) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT(RCAT("unknown error for ", GETRS) " in ", AX_EQ_B_LU) "()\n");+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }++ /* copy the result in x */+ for(i=0; i<m; i++){+ x[i]=b[i];+ }++#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 1;+}++/*+ * This function returns the solution of Ax = b+ *+ * The function is based on SVD decomposition:+ * If A=U D V^T with U, V orthogonal and D diagonal, the linear system becomes+ * (U D V^T) x = b or x=V D^{-1} U^T b+ * Note that V D^{-1} U^T is the pseudoinverse A^++ *+ * A is mxm, b is mx1.+ *+ * The function returns 0 in case of error, 1 if successful+ *+ * This function is often called repetitively to solve problems of identical+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is+ * retained between calls and free'd-malloc'ed when not of the appropriate size.+ * A call with NULL as the first argument forces this memory to be released.+ */+int AX_EQ_B_SVD(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)+{+__STATIC__ LM_REAL *buf=NULL;+__STATIC__ int buf_sz=0;+static LM_REAL eps=LM_CNST(-1.0);++register int i, j;+LM_REAL *a, *u, *s, *vt, *work;+int a_sz, u_sz, s_sz, vt_sz, tot_sz;+LM_REAL thresh, one_over_denom;+register LM_REAL sum;+int info, rank, worksz, *iwork, iworksz;++ if(!A)+#ifdef LINSOLVERS_RETAIN_MEMORY+ {+ if(buf) free(buf);+ buf=NULL;+ buf_sz=0;++ return 1;+ }+#else+ return 1; /* NOP */+#endif /* LINSOLVERS_RETAIN_MEMORY */++ /* calculate required memory size */+#if 1 /* use optimal size */+ worksz=-1; // workspace query. Keep in mind that GESDD requires more memory than GESVD+ /* note that optimal work size is returned in thresh */+ GESVD("A", "A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m, (LM_REAL *)&thresh, (int *)&worksz, &info);+ //GESDD("A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m, (LM_REAL *)&thresh, (int *)&worksz, NULL, &info);+ worksz=(int)thresh;+#else /* use minimum size */+ worksz=5*m; // min worksize for GESVD+ //worksz=m*(7*m+4); // min worksize for GESDD+#endif+ iworksz=8*m;+ a_sz=m*m;+ u_sz=m*m; s_sz=m; vt_sz=m*m;++ tot_sz=(a_sz + u_sz + s_sz + vt_sz + worksz)*sizeof(LM_REAL) + iworksz*sizeof(int); /* should be arranged in that order for proper doubles alignment */++#ifdef LINSOLVERS_RETAIN_MEMORY+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */+ if(buf) free(buf); /* free previously allocated memory */++ buf_sz=tot_sz;+ buf=(LM_REAL *)malloc(buf_sz);+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_SVD) "() failed!\n");+ exit(1);+ }+ }+#else+ buf_sz=tot_sz;+ buf=(LM_REAL *)malloc(buf_sz);+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_SVD) "() failed!\n");+ exit(1);+ }+#endif /* LINSOLVERS_RETAIN_MEMORY */++ a=buf;+ u=a+a_sz;+ s=u+u_sz;+ vt=s+s_sz;+ work=vt+vt_sz;+ iwork=(int *)(work+worksz);++ /* store A (column major!) into a */+ for(i=0; i<m; i++)+ for(j=0; j<m; j++)+ a[i+j*m]=A[i*m+j];++ /* SVD decomposition of A */+ GESVD("A", "A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, &info);+ //GESDD("A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, iwork, &info);++ /* error treatment */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GESVD), "/" GESDD) " in ", AX_EQ_B_SVD) "()\n", -info);+ exit(1);+ }+ else{+ PRINT_ERROR(RCAT("LAPACK error: dgesdd (dbdsdc)/dgesvd (dbdsqr) failed to converge in ", AX_EQ_B_SVD) "() [info=%d]\n", info);+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ }++ if(eps<0.0){+ LM_REAL aux;++ /* compute machine epsilon */+ for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))+ ;+ eps*=LM_CNST(2.0);+ }++ /* compute the pseudoinverse in a */+ for(i=0; i<a_sz; i++) a[i]=0.0; /* initialize to zero */+ for(rank=0, thresh=eps*s[0]; rank<m && s[rank]>thresh; rank++){+ one_over_denom=LM_CNST(1.0)/s[rank];++ for(j=0; j<m; j++)+ for(i=0; i<m; i++)+ a[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;+ }++ /* compute A^+ b in x */+ for(i=0; i<m; i++){+ for(j=0, sum=0.0; j<m; j++)+ sum+=a[i*m+j]*B[j];+ x[i]=sum;+ }++#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 1;+}++/* undefine all. IT MUST REMAIN IN THIS POSITION IN FILE */+#undef AX_EQ_B_QR+#undef AX_EQ_B_QRLS+#undef AX_EQ_B_CHOL+#undef AX_EQ_B_LU+#undef AX_EQ_B_SVD++#undef GEQRF+#undef ORGQR+#undef TRTRS+#undef POTF2+#undef POTRF+#undef POTRS+#undef GETRF+#undef GETRS+#undef GESVD+#undef GESDD++#else // no LAPACK++/* precision-specific definitions */+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU_noLapack)++/*+ * This function returns the solution of Ax = b+ *+ * The function employs LU decomposition followed by forward/back substitution (see+ * also the LAPACK-based LU solver above)+ *+ * A is mxm, b is mx1+ *+ * The function returns 0 in case of error, 1 if successful+ *+ * This function is often called repetitively to solve problems of identical+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is+ * retained between calls and free'd-malloc'ed when not of the appropriate size.+ * A call with NULL as the first argument forces this memory to be released.+ */+int AX_EQ_B_LU(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)+{+__STATIC__ void *buf=NULL;+__STATIC__ int buf_sz=0;++register int i, j, k;+int *idx, maxi=-1, idx_sz, a_sz, work_sz, tot_sz;+LM_REAL *a, *work, max, sum, tmp;++ if(!A)+#ifdef LINSOLVERS_RETAIN_MEMORY+ {+ if(buf) free(buf);+ buf=NULL;+ buf_sz=0;++ return 1;+ }+#else+ return 1; /* NOP */+#endif /* LINSOLVERS_RETAIN_MEMORY */++ /* calculate required memory size */+ idx_sz=m;+ a_sz=m*m;+ work_sz=m;+ tot_sz=(a_sz+work_sz)*sizeof(LM_REAL) + idx_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */++#ifdef LINSOLVERS_RETAIN_MEMORY+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */+ if(buf) free(buf); /* free previously allocated memory */++ buf_sz=tot_sz;+ buf=(void *)malloc(tot_sz);+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");+ exit(1);+ }+ }+#else+ buf_sz=tot_sz;+ buf=(void *)malloc(tot_sz);+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");+ exit(1);+ }+#endif /* LINSOLVERS_RETAIN_MEMORY */++ a=buf;+ work=a+a_sz;+ idx=(int *)(work+work_sz);++ /* avoid destroying A, B by copying them to a, x resp. */+ for(i=0; i<m; ++i){ // B & 1st row of A+ a[i]=A[i];+ x[i]=B[i];+ }+ for( ; i<a_sz; ++i) a[i]=A[i]; // copy A's remaining rows+ /****+ for(i=0; i<m; ++i){+ for(j=0; j<m; ++j)+ a[i*m+j]=A[i*m+j];+ x[i]=B[i];+ }+ ****/++ /* compute the LU decomposition of a row permutation of matrix a; the permutation itself is saved in idx[] */+ for(i=0; i<m; ++i){+ max=0.0;+ for(j=0; j<m; ++j)+ if((tmp=FABS(a[i*m+j]))>max)+ max=tmp;+ if(max==0.0){+ PRINT_ERROR(RCAT("Singular matrix A in ", AX_EQ_B_LU) "()!\n");+#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 0;+ }+ work[i]=LM_CNST(1.0)/max;+ }++ for(j=0; j<m; ++j){+ for(i=0; i<j; ++i){+ sum=a[i*m+j];+ for(k=0; k<i; ++k)+ sum-=a[i*m+k]*a[k*m+j];+ a[i*m+j]=sum;+ }+ max=0.0;+ for(i=j; i<m; ++i){+ sum=a[i*m+j];+ for(k=0; k<j; ++k)+ sum-=a[i*m+k]*a[k*m+j];+ a[i*m+j]=sum;+ if((tmp=work[i]*FABS(sum))>=max){+ max=tmp;+ maxi=i;+ }+ }+ if(j!=maxi){+ for(k=0; k<m; ++k){+ tmp=a[maxi*m+k];+ a[maxi*m+k]=a[j*m+k];+ a[j*m+k]=tmp;+ }+ work[maxi]=work[j];+ }+ idx[j]=maxi;+ if(a[j*m+j]==0.0)+ a[j*m+j]=LM_REAL_EPSILON;+ if(j!=m-1){+ tmp=LM_CNST(1.0)/(a[j*m+j]);+ for(i=j+1; i<m; ++i)+ a[i*m+j]*=tmp;+ }+ }++ /* The decomposition has now replaced a. Solve the linear system using+ * forward and back substitution+ */+ for(i=k=0; i<m; ++i){+ j=idx[i];+ sum=x[j];+ x[j]=x[i];+ if(k!=0)+ for(j=k-1; j<i; ++j)+ sum-=a[i*m+j]*x[j];+ else+ if(sum!=0.0)+ k=i+1;+ x[i]=sum;+ }++ for(i=m-1; i>=0; --i){+ sum=x[i];+ for(j=i+1; j<m; ++j)+ sum-=a[i*m+j]*x[j];+ x[i]=sum/a[i*m+i];+ }++#ifndef LINSOLVERS_RETAIN_MEMORY+ free(buf);+#endif++ return 1;+}++/* undefine all. IT MUST REMAIN IN THIS POSITION IN FILE */+#undef AX_EQ_B_LU++#endif /* HAVE_LAPACK */
+ levmar-2.4/CMakeLists.txt view
@@ -0,0 +1,52 @@+# levmar CMake file; see http://www.cmake.org and +# http://www.insightsoftwareconsortium.org/wiki/index.php/CMake_Tutorial + +PROJECT(LEVMAR) +#CMAKE_MINIMUM_REQUIRED(VERSION 1.4) + +# compiler flags +ADD_DEFINITIONS(-DLINSOLVERS_RETAIN_MEMORY) # do not free memory between linear solvers calls +#REMOVE_DEFINITIONS(-DLINSOLVERS_RETAIN_MEMORY) + +# f2c is sometimes equivalent to libF77 & libI77; in that case, set HAVE_F2C to 0 +SET(HAVE_F2C 1 CACHE BOOL "Do we have f2c or F77/I77?" ) + +# the directory where the lapack/blas/f2c libraries reside +SET(LAPACKBLAS_DIR /usr/lib CACHE PATH "Path to lapack/blas libraries") + +# actual names for the lapack/blas/f2c libraries +SET(LAPACK_LIB lapack CACHE STRING "The name of the lapack library") +SET(BLAS_LIB blas CACHE STRING "The name of the blas library") +IF(HAVE_F2C) + SET(F2C_LIB f2c CACHE STRING "The name of the f2c library") +ELSE(HAVE_F2C) + SET(F77_LIB libF77 CACHE STRING "The name of the F77 library") + SET(I77_LIB libI77 CACHE STRING "The name of the I77 library") +ENDIF(HAVE_F2C) + +########################## NO CHANGES BEYOND THIS POINT ########################## + +#INCLUDE_DIRECTORIES(/usr/include) +LINK_DIRECTORIES(${LAPACKBLAS_DIR}) + +# levmar library source files +ADD_LIBRARY(levmar STATIC + lm.c Axb.c misc.c lmlec.c lmbc.c lmblec.c + lm.h misc.h compiler.h +) + +# demo program +ADD_EXECUTABLE(lmdemo lmdemo.c lm.h) +# libraries the demo depends on +IF(HAVE_F2C) + TARGET_LINK_LIBRARIES(lmdemo levmar ${LAPACK_LIB} ${BLAS_LIB} ${F2C_LIB}) +ELSE(HAVE_F2C) + TARGET_LINK_LIBRARIES(lmdemo levmar ${LAPACK_LIB} ${BLAS_LIB} ${F77_LIB} ${I77_LIB}) +ENDIF(HAVE_F2C) + +# make sure that the library is built before the demo +ADD_DEPENDENCIES(lmdemo levmar) + +#SUBDIRS(matlab) + +#ADD_TEST(levmar_tst lmdemo)
+ levmar-2.4/LICENSE view
@@ -0,0 +1,340 @@+ GNU GENERAL PUBLIC LICENSE+ Version 2, June 1991++ Copyright (C) 1989, 1991 Free Software Foundation, Inc.+ 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA+ Everyone is permitted to copy and distribute verbatim copies+ of this license document, but changing it is not allowed.++ Preamble++ The licenses for most software are designed to take away your+freedom to share and change it. By contrast, the GNU General Public+License is intended to guarantee your freedom to share and change free+software--to make sure the software is free for all its users. This+General Public License applies to most of the Free Software+Foundation's software and to any other program whose authors commit to+using it. (Some other Free Software Foundation software is covered by+the GNU Library General Public License instead.) You can apply it to+your programs, too.++ When we speak of free software, we are referring to freedom, not+price. Our General Public Licenses are designed to make sure that you+have the freedom to distribute copies of free software (and charge for+this service if you wish), that you receive source code or can get it+if you want it, that you can change the software or use pieces of it+in new free programs; and that you know you can do these things.++ To protect your rights, we need to make restrictions that forbid+anyone to deny you these rights or to ask you to surrender the rights.+These restrictions translate to certain responsibilities for you if you+distribute copies of the software, or if you modify it.++ For example, if you distribute copies of such a program, whether+gratis or for a fee, you must give the recipients all the rights that+you have. You must make sure that they, too, receive or can get the+source code. And you must show them these terms so they know their+rights.++ We protect your rights with two steps: (1) copyright the software, and+(2) offer you this license which gives you legal permission to copy,+distribute and/or modify the software.++ Also, for each author's protection and ours, we want to make certain+that everyone understands that there is no warranty for this free+software. If the software is modified by someone else and passed on, we+want its recipients to know that what they have is not the original, so+that any problems introduced by others will not reflect on the original+authors' reputations.++ Finally, any free program is threatened constantly by software+patents. We wish to avoid the danger that redistributors of a free+program will individually obtain patent licenses, in effect making the+program proprietary. To prevent this, we have made it clear that any+patent must be licensed for everyone's free use or not licensed at all.++ The precise terms and conditions for copying, distribution and+modification follow.++ GNU GENERAL PUBLIC LICENSE+ TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION++ 0. This License applies to any program or other work which contains+a notice placed by the copyright holder saying it may be distributed+under the terms of this General Public License. The "Program", below,+refers to any such program or work, and a "work based on the Program"+means either the Program or any derivative work under copyright law:+that is to say, a work containing the Program or a portion of it,+either verbatim or with modifications and/or translated into another+language. (Hereinafter, translation is included without limitation in+the term "modification".) Each licensee is addressed as "you".++Activities other than copying, distribution and modification are not+covered by this License; they are outside its scope. The act of+running the Program is not restricted, and the output from the Program+is covered only if its contents constitute a work based on the+Program (independent of having been made by running the Program).+Whether that is true depends on what the Program does.++ 1. You may copy and distribute verbatim copies of the Program's+source code as you receive it, in any medium, provided that you+conspicuously and appropriately publish on each copy an appropriate+copyright notice and disclaimer of warranty; keep intact all the+notices that refer to this License and to the absence of any warranty;+and give any other recipients of the Program a copy of this License+along with the Program.++You may charge a fee for the physical act of transferring a copy, and+you may at your option offer warranty protection in exchange for a fee.++ 2. You may modify your copy or copies of the Program or any portion+of it, thus forming a work based on the Program, and copy and+distribute such modifications or work under the terms of Section 1+above, provided that you also meet all of these conditions:++ a) You must cause the modified files to carry prominent notices+ stating that you changed the files and the date of any change.++ b) You must cause any work that you distribute or publish, that in+ whole or in part contains or is derived from the Program or any+ part thereof, to be licensed as a whole at no charge to all third+ parties under the terms of this License.++ c) If the modified program normally reads commands interactively+ when run, you must cause it, when started running for such+ interactive use in the most ordinary way, to print or display an+ announcement including an appropriate copyright notice and a+ notice that there is no warranty (or else, saying that you provide+ a warranty) and that users may redistribute the program under+ these conditions, and telling the user how to view a copy of this+ License. (Exception: if the Program itself is interactive but+ does not normally print such an announcement, your work based on+ the Program is not required to print an announcement.)++These requirements apply to the modified work as a whole. If+identifiable sections of that work are not derived from the Program,+and can be reasonably considered independent and separate works in+themselves, then this License, and its terms, do not apply to those+sections when you distribute them as separate works. But when you+distribute the same sections as part of a whole which is a work based+on the Program, the distribution of the whole must be on the terms of+this License, whose permissions for other licensees extend to the+entire whole, and thus to each and every part regardless of who wrote it.++Thus, it is not the intent of this section to claim rights or contest+your rights to work written entirely by you; rather, the intent is to+exercise the right to control the distribution of derivative or+collective works based on the Program.++In addition, mere aggregation of another work not based on the Program+with the Program (or with a work based on the Program) on a volume of+a storage or distribution medium does not bring the other work under+the scope of this License.++ 3. You may copy and distribute the Program (or a work based on it,+under Section 2) in object code or executable form under the terms of+Sections 1 and 2 above provided that you also do one of the following:++ a) Accompany it with the complete corresponding machine-readable+ source code, which must be distributed under the terms of Sections+ 1 and 2 above on a medium customarily used for software interchange; or,++ b) Accompany it with a written offer, valid for at least three+ years, to give any third party, for a charge no more than your+ cost of physically performing source distribution, a complete+ machine-readable copy of the corresponding source code, to be+ distributed under the terms of Sections 1 and 2 above on a medium+ customarily used for software interchange; or,++ c) Accompany it with the information you received as to the offer+ to distribute corresponding source code. (This alternative is+ allowed only for noncommercial distribution and only if you+ received the program in object code or executable form with such+ an offer, in accord with Subsection b above.)++The source code for a work means the preferred form of the work for+making modifications to it. For an executable work, complete source+code means all the source code for all modules it contains, plus any+associated interface definition files, plus the scripts used to+control compilation and installation of the executable. However, as a+special exception, the source code distributed need not include+anything that is normally distributed (in either source or binary+form) with the major components (compiler, kernel, and so on) of the+operating system on which the executable runs, unless that component+itself accompanies the executable.++If distribution of executable or object code is made by offering+access to copy from a designated place, then offering equivalent+access to copy the source code from the same place counts as+distribution of the source code, even though third parties are not+compelled to copy the source along with the object code.++ 4. You may not copy, modify, sublicense, or distribute the Program+except as expressly provided under this License. Any attempt+otherwise to copy, modify, sublicense or distribute the Program is+void, and will automatically terminate your rights under this License.+However, parties who have received copies, or rights, from you under+this License will not have their licenses terminated so long as such+parties remain in full compliance.++ 5. You are not required to accept this License, since you have not+signed it. However, nothing else grants you permission to modify or+distribute the Program or its derivative works. These actions are+prohibited by law if you do not accept this License. Therefore, by+modifying or distributing the Program (or any work based on the+Program), you indicate your acceptance of this License to do so, and+all its terms and conditions for copying, distributing or modifying+the Program or works based on it.++ 6. Each time you redistribute the Program (or any work based on the+Program), the recipient automatically receives a license from the+original licensor to copy, distribute or modify the Program subject to+these terms and conditions. You may not impose any further+restrictions on the recipients' exercise of the rights granted herein.+You are not responsible for enforcing compliance by third parties to+this License.++ 7. If, as a consequence of a court judgment or allegation of patent+infringement or for any other reason (not limited to patent issues),+conditions are imposed on you (whether by court order, agreement or+otherwise) that contradict the conditions of this License, they do not+excuse you from the conditions of this License. If you cannot+distribute so as to satisfy simultaneously your obligations under this+License and any other pertinent obligations, then as a consequence you+may not distribute the Program at all. For example, if a patent+license would not permit royalty-free redistribution of the Program by+all those who receive copies directly or indirectly through you, then+the only way you could satisfy both it and this License would be to+refrain entirely from distribution of the Program.++If any portion of this section is held invalid or unenforceable under+any particular circumstance, the balance of the section is intended to+apply and the section as a whole is intended to apply in other+circumstances.++It is not the purpose of this section to induce you to infringe any+patents or other property right claims or to contest validity of any+such claims; this section has the sole purpose of protecting the+integrity of the free software distribution system, which is+implemented by public license practices. Many people have made+generous contributions to the wide range of software distributed+through that system in reliance on consistent application of that+system; it is up to the author/donor to decide if he or she is willing+to distribute software through any other system and a licensee cannot+impose that choice.++This section is intended to make thoroughly clear what is believed to+be a consequence of the rest of this License.++ 8. If the distribution and/or use of the Program is restricted in+certain countries either by patents or by copyrighted interfaces, the+original copyright holder who places the Program under this License+may add an explicit geographical distribution limitation excluding+those countries, so that distribution is permitted only in or among+countries not thus excluded. In such case, this License incorporates+the limitation as if written in the body of this License.++ 9. The Free Software Foundation may publish revised and/or new versions+of the General Public License from time to time. Such new versions will+be similar in spirit to the present version, but may differ in detail to+address new problems or concerns.++Each version is given a distinguishing version number. If the Program+specifies a version number of this License which applies to it and "any+later version", you have the option of following the terms and conditions+either of that version or of any later version published by the Free+Software Foundation. If the Program does not specify a version number of+this License, you may choose any version ever published by the Free Software+Foundation.++ 10. If you wish to incorporate parts of the Program into other free+programs whose distribution conditions are different, write to the author+to ask for permission. For software which is copyrighted by the Free+Software Foundation, write to the Free Software Foundation; we sometimes+make exceptions for this. Our decision will be guided by the two goals+of preserving the free status of all derivatives of our free software and+of promoting the sharing and reuse of software generally.++ NO WARRANTY++ 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY+FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN+OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES+PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED+OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF+MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS+TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE+PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,+REPAIR OR CORRECTION.++ 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR+REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,+INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING+OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED+TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY+YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER+PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE+POSSIBILITY OF SUCH DAMAGES.++ END OF TERMS AND CONDITIONS++ How to Apply These Terms to Your New Programs++ If you develop a new program, and you want it to be of the greatest+possible use to the public, the best way to achieve this is to make it+free software which everyone can redistribute and change under these terms.++ To do so, attach the following notices to the program. It is safest+to attach them to the start of each source file to most effectively+convey the exclusion of warranty; and each file should have at least+the "copyright" line and a pointer to where the full notice is found.++ <one line to give the program's name and a brief idea of what it does.>+ Copyright (C) <year> <name of author>++ This program is free software; you can redistribute it and/or modify+ it under the terms of the GNU General Public License as published by+ the Free Software Foundation; either version 2 of the License, or+ (at your option) any later version.++ This program is distributed in the hope that it will be useful,+ but WITHOUT ANY WARRANTY; without even the implied warranty of+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+ GNU General Public License for more details.++ You should have received a copy of the GNU General Public License+ along with this program; if not, write to the Free Software+ Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA+++Also add information on how to contact you by electronic and paper mail.++If the program is interactive, make it output a short notice like this+when it starts in an interactive mode:++ Gnomovision version 69, Copyright (C) year name of author+ Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.+ This is free software, and you are welcome to redistribute it+ under certain conditions; type `show c' for details.++The hypothetical commands `show w' and `show c' should show the appropriate+parts of the General Public License. Of course, the commands you use may+be called something other than `show w' and `show c'; they could even be+mouse-clicks or menu items--whatever suits your program.++You should also get your employer (if you work as a programmer) or your+school, if any, to sign a "copyright disclaimer" for the program, if+necessary. Here is a sample; alter the names:++ Yoyodyne, Inc., hereby disclaims all copyright interest in the program+ `Gnomovision' (which makes passes at compilers) written by James Hacker.++ <signature of Ty Coon>, 1 April 1989+ Ty Coon, President of Vice++This General Public License does not permit incorporating your program into+proprietary programs. If your program is a subroutine library, you may+consider it more useful to permit linking proprietary applications with the+library. If this is what you want to do, use the GNU Library General+Public License instead of this License.
+ levmar-2.4/Makefile view
@@ -0,0 +1,62 @@+#+# Unix/Linux GCC Makefile for Levenberg - Marquardt minimization+# Under windows, use Makefile.vc for MSVC+#++CC=gcc+CONFIGFLAGS=#-ULINSOLVERS_RETAIN_MEMORY+#ARCHFLAGS=-march=pentium4 # YOU MIGHT WANT TO UNCOMMENT THIS FOR P4+CFLAGS=$(CONFIGFLAGS) $(ARCHFLAGS) -O3 -funroll-loops -Wall #-pg+LAPACKLIBS_PATH=/usr/local/lib # WHEN USING LAPACK, CHANGE THIS TO WHERE YOUR COMPILED LIBS ARE!+LDFLAGS=-L$(LAPACKLIBS_PATH) -L.+LIBOBJS=lm.o Axb.o misc.o lmlec.o lmbc.o lmblec.o+LIBSRCS=lm.c Axb.c misc.c lmlec.c lmbc.c lmblec.c+DEMOBJS=lmdemo.o+DEMOSRCS=lmdemo.c+AR=ar+RANLIB=ranlib+LAPACKLIBS=-llapack -lblas -lf2c # comment this line if you are not using LAPACK.+ # On systems with a FORTRAN (not f2c'ed) version of LAPACK, -lf2c is+ # not necessary; on others, -lf2c is equivalent to -lF77 -lI77++#LAPACKLIBS=-L/usr/local/atlas/lib -llapack -lcblas -lf77blas -latlas -lf2c # This works with the ATLAS updated lapack and Linux_P4SSE2+ # from http://www.netlib.org/atlas/archives/linux/++#LAPACKLIBS=-llapack -lgoto -lpthread -lf2c # This works with GotoBLAS+ # from http://www.tacc.utexas.edu/resources/software/++#LAPACKLIBS=-L/opt/intel/mkl/8.0.1/lib/32/ -lmkl_lapack -lmkl_ia32 -lguide -lf2c # This works with MKL 8.0.1 from+ # http://www.intel.com/cd/software/products/asmo-na/eng/perflib/mkl/index.htm++LIBS=$(LAPACKLIBS)++all: liblevmar.a lmdemo++liblevmar.a: $(LIBOBJS)+ $(AR) crv liblevmar.a $(LIBOBJS)+ $(RANLIB) liblevmar.a++lmdemo: $(DEMOBJS) liblevmar.a+ $(CC) $(LDFLAGS) $(DEMOBJS) -o lmdemo -llevmar $(LIBS) -lm++lm.o: lm.c lm_core.c lm.h misc.h compiler.h+Axb.o: Axb.c Axb_core.c lm.h misc.h+misc.o: misc.c misc_core.c lm.h misc.h+lmlec.o: lmlec.c lmlec_core.c lm.h misc.h+lmbc.o: lmbc.c lmbc_core.c lm.h misc.h compiler.h+lmblec.o: lmblec.c lmblec_core.c lm.h misc.h++lmdemo.o: lm.h++clean:+ @rm -f $(LIBOBJS) $(DEMOBJS)++cleanall: clean+ @rm -f lmdemo+ @rm -f liblevmar.a++depend:+ makedepend -f Makefile $(LIBSRCS) $(DEMOSRCS)++# DO NOT DELETE THIS LINE -- make depend depends on it.+
+ levmar-2.4/Makefile.icc view
@@ -0,0 +1,58 @@+#+# Unix/Linux Intel ICC Makefile for Levenberg - Marquardt minimization+# To be used with "make -f Makefile.icc"+# Under windows, use Makefile.vc for MSVC+#++CC=icc #-w1 # warnings on+CXX=icpc+CONFIGFLAGS=#-ULINSOLVERS_RETAIN_MEMORY+ARCHFLAGS=-march=pentium4 -mcpu=pentium4+CFLAGS=$(CONFIGFLAGS) $(ARCHFLAGS) -O3 -tpp7 -xW -ip -ipo -unroll #-g+LAPACKLIBS_PATH=/usr/local/lib # WHEN USING LAPACK, CHANGE THIS TO WHERE YOUR COMPILED LIBS ARE!+LDFLAGS=-L$(LAPACKLIBS_PATH) -L.+LIBOBJS=lm.o Axb.o misc.o lmlec.o lmbc.o lmblec.o+LIBSRCS=lm.c Axb.c misc.c lmlec.c lmbc.c lmblec.c+DEMOBJS=lmdemo.o+DEMOSRCS=lmdemo.c+AR=xiar+#RANLIB=ranlib+LAPACKLIBS=-llapack -lblas -lf2c # comment this line if you are not using LAPACK.+ # On systems with a FORTRAN (not f2c'ed) version of LAPACK, -lf2c is+ # not necessary; on others, -lf2c is equivalent to -lF77 -lI77++# The following works with the ATLAS updated lapack and Linux_P4SSE2 from http://www.netlib.org/atlas/archives/linux/+#LAPACKLIBS=-L/usr/local/atlas/lib -llapack -lcblas -lf77blas -latlas -lf2c++LIBS=$(LAPACKLIBS)++all: liblevmar.a lmdemo++liblevmar.a: $(LIBOBJS)+ $(AR) crv liblevmar.a $(LIBOBJS)+ #$(RANLIB) liblevmar.a++lmdemo: $(DEMOBJS) liblevmar.a+ $(CC) $(ARCHFLAGS) $(LDFLAGS) $(DEMOBJS) -o lmdemo -llevmar $(LIBS) -lm++lm.o: lm.c lm_core.c lm.h misc.h compiler.h+Axb.o: Axb.c Axb_core.c lm.h misc.h+misc.o: misc.c misc_core.c lm.h misc.h+lmlec.o: lmlec.c lmlec_core.c lm.h misc.h+lmbc.o: lmbc.c lmbc_core.c lm.h misc.h compiler.h+lmblec.o: lmblec.c lmblec_core.c lm.h misc.h++lmdemo.o: lm.h++clean:+ @rm -f $(LIBOBJS) $(DEMOBJS)++cleanall: clean+ @rm -f lmdemo+ @rm -f liblevmar.a++depend:+ makedepend -f Makefile.icc $(LIBSRCS) $(DEMOSRCS)++# DO NOT DELETE THIS LINE -- make depend depends on it.+
+ levmar-2.4/Makefile.vc view
@@ -0,0 +1,58 @@+#+# MS Visual C Makefile for Levenberg - Marquardt minimization+# Under Unix/Linux, use Makefile for GCC+#+# At the command prompt, type+# nmake /f Makefile.vc+#+# NOTE: To use this, you must have MSVC installed and properly+# configured for command line use (you might need to run VCVARS32.BAT+# included with your copy of MSVC). Another option is to use the+# free MSVC toolkit from http://msdn.microsoft.com/visualc/vctoolkit2003/+#++MAKE=nmake /nologo+CC=cl /nologo+CONFIGFLAGS=#/ULINSOLVERS_RETAIN_MEMORY+# YOU MIGHT WANT TO UNCOMMENT THE FOLLOWING LINE+#SPOPTFLAGS=/GL /G7 /arch:SSE2 # special optimization: resp. whole program opt., Athlon/Pentium4 opt., SSE2 extensions+# /MD COMPILES WITH MULTIPLE THREADS SUPPORT. TO DISABLE IT, SUBSTITUTE WITH /ML+# FLAG /EHsc SUPERSEDED /GX IN MSVC'05. IF YOU HAVE AN EARLIER VERSION THAT COMPLAINS ABOUT IT, CHANGE /EHsc TO /GX+CFLAGS=$(CONFIGFLAGS) /I. /MD /W3 /EHsc /O2 $(SPOPTFLAGS) # /Wall+LAPACKLIBS_PATH=C:\src\lib # WHEN USING LAPACK, CHANGE THIS TO WHERE YOUR COMPILED LIBS ARE!+LDFLAGS=/link /subsystem:console /opt:ref /libpath:$(LAPACKLIBS_PATH) /libpath:.+LIBOBJS=lm.obj Axb.obj misc.obj lmlec.obj lmbc.obj lmblec.obj+LIBSRCS=lm.c Axb.c misc.c lmlec.c lmbc.c lmblec.c+DEMOBJS=lmdemo.obj+DEMOSRCS=lmdemo.c+AR=lib /nologo++# comment the following line if you are not using LAPACK+LAPACKLIBS=clapack.lib blas.lib libF77.lib libI77.lib++LIBS=levmar.lib $(LAPACKLIBS)++all: levmar.lib lmdemo.exe++levmar.lib: $(LIBOBJS)+ $(AR) /out:levmar.lib $(LIBOBJS)++lmdemo.exe: $(DEMOBJS) levmar.lib+ $(CC) $(DEMOBJS) $(LDFLAGS) /out:lmdemo.exe $(LIBS)++lm.obj: lm.c lm_core.c lm.h misc.h compiler.h+Axb.obj: Axb.c Axb_core.c lm.h misc.h+misc.obj: misc.c misc_core.c lm.h misc.h+lmlec.obj: lmlec.c lmlec_core.c lm.h misc.h+lmbc.obj: lmbc.c lmbc_core.c lm.h misc.h compiler.h+lmblec.obj: lmblec.c lmblec_core.c lm.h misc.h++lmdemo.obj: lm.h++clean:+ -del $(LIBOBJS) $(DEMOBJS)++cleanall: clean+ -del lmdemo.exe+ -del levmar.lib+
+ levmar-2.4/README.txt view
@@ -0,0 +1,74 @@+ ************************************************************** + LEVMAR + version 2.4 + By Manolis Lourakis + + Institute of Computer Science + Foundation for Research and Technology - Hellas + Heraklion, Crete, Greece + ************************************************************** + + +GENERAL +This is levmar, a copylefted C/C++ implementation of the Levenberg-Marquardt non-linear +least squares algorithm. levmar includes double and single precision LM versions, both +with analytic and finite difference approximated jacobians. levmar also has some support +for constrained non-linear least squares, allowing linear equation and box constraints. +You have the following options regarding the solution of the underlying augmented normal +equations: + +1) Assuming that you have LAPACK (or an equivalent vendor library such as ESSL, MKL, + NAG, ...) installed, you can use the included LAPACK-based solvers (default). + +2) If you don't have LAPACK or decide not to use it, undefine HAVE_LAPACK in lm.h + and a LAPACK-free, LU-based linear systems solver will by used. Also, the line + setting the variable LAPACKLIBS in the Makefile should be commented out. + +It is strongly recommended that you *do* employ LAPACK; if you don't have it already, +I suggest getting clapack from http://www.netlib.org/clapack. However, LAPACK's +use is not mandatory and the 2nd option makes levmar totally self-contained. +See lmdemo.c for examples of use and http://www.ics.forth.gr/~lourakis/levmar +for general comments. An example of using levmar for data fitting is in expfit.c + +The mathematical theory behind levmar is described in the lecture notes entitled +"Methods for Non-Linear Least Squares Problems", by K. Madsen, H.B. Nielsen and O. Tingleff, +Technical University of Denmark (http://www.imm.dtu.dk/courses/02611/nllsq.pdf). + +LICENSE +levmar is released under the GNU Public License (GPL), which can be found in the included +LICENSE file. Note that under the terms of GPL, commercial use is allowed only if a software +employing levmar is also published in source under the GPL. However, if you are interested +in using levmar in a proprietary commercial apprlication, a commercial license for levmar +can be obtained by contacting the author using the email address at the end of this file. + +COMPILATION + - You might first consider setting a few configuration options at the top of + lm.h. See the accompanying comments for more details. + + - On a Linux/Unix system, typing "make" will build both levmar and the demo + program using gcc. Alternatively, if Intel's C++ compiler is installed, it + can be used by typing "make -f Makefile.icc". + + - Under Windows and if Visual C is installed & configured for command line + use, type "nmake /f Makefile.vc" in a cmd window to build levmar and the + demo program. In case of trouble, read the comments on top of Makefile.vc + Visual C++ project files (levmar.vcproj and lmdemo.vcproj) are also included, + however they are not supported and are only meant to serve as a starting point + for creating your own. Check http://www.arstdesign.com/articles/prjconverter.html + if you need to convert to .dsw/.dsp (i.e., Visual C++ 6.0) project files. + + - levmar can also be built under various platforms using the CMake cross-platform + build system. The included CMakeLists.txt file can be used to generate makefiles + for Unix systems or project files for Windows systems. See http://www.cmake.org + for details. + +MATLAB INTERFACE +Since version 2.2, the levmar distrubution includes a matlab interface. +See the 'matlab' subdirectory for more information and examples of use. + +Notice that *_core.c files are not to be compiled directly; For example, +Axb_core.c is included by Axb.c, to provide single and double precision +routine versions. + + +Send your comments/bug reports to lourakis at ics forth gr
+ levmar-2.4/compiler.h view
@@ -0,0 +1,41 @@+/////////////////////////////////////////////////////////////////////////////////+// +// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++#ifndef _COMPILER_H_+#define _COMPILER_H_++/* note: intel's icc defines both __ICC & __INTEL_COMPILER.+ * Also, some compilers other than gcc define __GNUC__,+ * therefore gcc should be checked last+ */+#ifdef _MSC_VER+#define inline __inline // MSVC+#elif !defined(__ICC) && !defined(__INTEL_COMPILER) && !defined(__GNUC__)+#define inline // other than MSVC, ICC, GCC: define empty+#endif++#ifdef _MSC_VER+#define LM_FINITE _finite // MSVC+#elif defined(__ICC) || defined(__INTEL_COMPILER) || defined(__GNUC__)+#define LM_FINITE finite // ICC, GCC+#else+#define LM_FINITE finite // other than MSVC, ICC, GCC, let's hope this will work+#endif ++#endif /* _COMPILER_H_ */
+ levmar-2.4/expfit.c view
@@ -0,0 +1,122 @@+////////////////////////////////////////////////////////////////////////////////////+// Example program that shows how to use levmar in order to fit the three-+// parameter exponential model x_i = p[0]*exp(-p[1]*i) + p[2] to a set of+// data measurements; example is based on a similar one from GSL.+//+// Copyright (C) 2008 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+////////////////////////////////////////////////////////////////////////////////////++#include <stdio.h>+#include <stdlib.h>+#include <math.h>++#include <lm.h>++#ifndef LM_DBL_PREC+#error Example program assumes that levmar has been compiled with double precision, see LM_DBL_PREC!+#endif+++/* the following macros concern the initialization of a random number generator for adding noise */+#undef REPEATABLE_RANDOM+#define DBL_RAND_MAX (double)(RAND_MAX)++#ifdef _MSC_VER // MSVC+#include <process.h>+#define GETPID _getpid+#elif defined(__GNUC__) // GCC+#include <sys/types.h>+#include <unistd.h>+#define GETPID getpid+#else+#warning Do not know the name of the function returning the process id for your OS/compiler combination+#define GETPID 0+#endif /* _MSC_VER */++#ifdef REPEATABLE_RANDOM+#define INIT_RANDOM(seed) srandom(seed)+#else+#define INIT_RANDOM(seed) srandom((int)GETPID()) // seed unused+#endif++/* Gaussian noise with mean m and variance s, uses the Box-Muller transformation */+double gNoise(double m, double s)+{+double r1, r2, val;++ r1=((double)random())/DBL_RAND_MAX;+ r2=((double)random())/DBL_RAND_MAX;++ val=sqrt(-2.0*log(r1))*cos(2.0*M_PI*r2);++ val=s*val+m;++ return val;+}++/* model to be fitted to measurements: x_i = p[0]*exp(-p[1]*i) + p[2], i=0...n-1 */+void expfunc(double *p, double *x, int m, int n, void *data)+{+register int i;++ for(i=0; i<n; ++i){+ x[i]=p[0]*exp(-p[1]*i) + p[2];+ }+}++/* Jacobian of expfunc() */+void jacexpfunc(double *p, double *jac, int m, int n, void *data)+{ +register int i, j;+ + /* fill Jacobian row by row */+ for(i=j=0; i<n; ++i){+ jac[j++]=exp(-p[1]*i);+ jac[j++]=-p[0]*i*exp(-p[1]*i);+ jac[j++]=1.0;+ }+}++int main()+{+const int n=40, m=3; // 40 measurements, 3 parameters+double p[m], x[n], opts[LM_OPTS_SZ], info[LM_INFO_SZ];+register int i;+int ret;++ /* generate some measurement using the exponential model with+ * parameters (5.0, 0.1, 1.0), corrupted with zero-mean+ * Gaussian noise of s=0.1+ */+ INIT_RANDOM(0);+ for(i=0; i<n; ++i)+ x[i]=(5.0*exp(-0.1*i) + 1.0) + gNoise(0.0, 0.1);++ /* initial parameters estimate: (1.0, 0.0, 0.0) */+ p[0]=1.0; p[1]=0.0; p[2]=0.0;++ /* optimization control parameters; passing to levmar NULL instead of opts reverts to defaults */+ opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20;+ opts[4]=LM_DIFF_DELTA; // relevant only if the finite difference Jacobian version is used ++ /* invoke the optimization function */+ ret=dlevmar_der(expfunc, jacexpfunc, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ //ret=dlevmar_dif(expfunc, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // without Jacobian+ printf("Levenberg-Marquardt returned in %g iter, reason %g, sumsq %g [%g]\n", info[5], info[6], info[1], info[0]);+ printf("Best fit parameters: %.7g %.7g %.7g\n", p[0], p[1], p[2]);++ exit(0);+}
+ levmar-2.4/levmar.vcproj view
@@ -0,0 +1,196 @@+<?xml version="1.0" encoding="UTF-8"?> +<VisualStudioProject + ProjectType="Visual C++" + Version="8,00" + Name="levmar" + ProjectGUID="{F329E490-DB04-453A-A0BF-FEB90BD949D8}" + Keyword="Win32Proj" + > + <Platforms> + <Platform + Name="Win32" + /> + </Platforms> + <ToolFiles> + </ToolFiles> + <Configurations> + <Configuration + Name="Debug|Win32" + OutputDirectory="Debug" + IntermediateDirectory="Debug" + ConfigurationType="4" + > + <Tool + Name="VCPreBuildEventTool" + /> + <Tool + Name="VCCustomBuildTool" + /> + <Tool + Name="VCXMLDataGeneratorTool" + /> + <Tool + Name="VCWebServiceProxyGeneratorTool" + /> + <Tool + Name="VCMIDLTool" + /> + <Tool + Name="VCCLCompilerTool" + Optimization="0" + PreprocessorDefinitions="WIN32;_DEBUG;_CONSOLE" + MinimalRebuild="true" + BasicRuntimeChecks="3" + RuntimeLibrary="3" + UsePrecompiledHeader="0" + WarningLevel="3" + Detect64BitPortabilityProblems="true" + DebugInformationFormat="4" + CompileAs="1" + /> + <Tool + Name="VCManagedResourceCompilerTool" + /> + <Tool + Name="VCResourceCompilerTool" + /> + <Tool + Name="VCPreLinkEventTool" + /> + <Tool + Name="VCLibrarianTool" + /> + <Tool + Name="VCALinkTool" + /> + <Tool + Name="VCXDCMakeTool" + /> + <Tool + Name="VCBscMakeTool" + /> + <Tool + Name="VCFxCopTool" + /> + <Tool + Name="VCPostBuildEventTool" + /> + </Configuration> + <Configuration + Name="Release|Win32" + OutputDirectory="Release" + IntermediateDirectory="Release" + ConfigurationType="4" + > + <Tool + Name="VCPreBuildEventTool" + /> + <Tool + Name="VCCustomBuildTool" + /> + <Tool + Name="VCXMLDataGeneratorTool" + /> + <Tool + Name="VCWebServiceProxyGeneratorTool" + /> + <Tool + Name="VCMIDLTool" + /> + <Tool + Name="VCCLCompilerTool" + PreprocessorDefinitions="WIN32;NDEBUG;_CONSOLE" + RuntimeLibrary="2" + UsePrecompiledHeader="0" + WarningLevel="3" + Detect64BitPortabilityProblems="true" + DebugInformationFormat="3" + CompileAs="1" + /> + <Tool + Name="VCManagedResourceCompilerTool" + /> + <Tool + Name="VCResourceCompilerTool" + /> + <Tool + Name="VCPreLinkEventTool" + /> + <Tool + Name="VCLibrarianTool" + /> + <Tool + Name="VCALinkTool" + /> + <Tool + Name="VCXDCMakeTool" + /> + <Tool + Name="VCBscMakeTool" + /> + <Tool + Name="VCFxCopTool" + /> + <Tool + Name="VCPostBuildEventTool" + /> + </Configuration> + </Configurations> + <References> + </References> + <Files> + <Filter + Name="Header Files" + Filter="h;hpp;hxx;hm;inl;inc;xsd" + > + <File + RelativePath=".\compiler.h" + > + </File> + <File + RelativePath=".\lm.h" + > + </File> + <File + RelativePath=".\misc.h" + > + </File> + </Filter> + <Filter + Name="Resource Files" + Filter="rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx" + > + </Filter> + <Filter + Name="Source Files" + Filter="cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx" + > + <File + RelativePath=".\Axb.c" + > + </File> + <File + RelativePath=".\lm.c" + > + </File> + <File + RelativePath=".\lmbc.c" + > + </File> + <File + RelativePath=".\lmblec.c" + > + </File> + <File + RelativePath=".\lmlec.c" + > + </File> + <File + RelativePath=".\misc.c" + > + </File> + </Filter> + </Files> + <Globals> + </Globals> +</VisualStudioProject>
+ levmar-2.4/lm.c view
@@ -0,0 +1,83 @@+/////////////////////////////////////////////////////////////////////////////////+// +// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++/******************************************************************************** + * Levenberg-Marquardt nonlinear minimization. The same core code is used with+ * appropriate #defines to derive single and double precision versions, see+ * also lm_core.c+ ********************************************************************************/++#include <stdio.h>+#include <stdlib.h>+#include <math.h>+#include <float.h>++#include "lm.h"+#include "compiler.h"+#include "misc.h"++#define EPSILON 1E-12+#define ONE_THIRD 0.3333333334 /* 1.0/3.0 */++#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!+#endif+++#ifdef LM_SNGL_PREC+/* single precision (float) definitions */+#define LM_REAL float+#define LM_PREFIX s++#define LM_REAL_MAX FLT_MAX+#define LM_REAL_MIN -FLT_MAX+#define LM_REAL_EPSILON FLT_EPSILON+#define __SUBCNST(x) x##F+#define LM_CNST(x) __SUBCNST(x) // force substitution++#include "lm_core.c" // read in core code++#undef LM_REAL+#undef LM_PREFIX+#undef LM_REAL_MAX+#undef LM_REAL_EPSILON+#undef LM_REAL_MIN+#undef __SUBCNST+#undef LM_CNST+#endif /* LM_SNGL_PREC */++#ifdef LM_DBL_PREC+/* double precision definitions */+#define LM_REAL double+#define LM_PREFIX d++#define LM_REAL_MAX DBL_MAX+#define LM_REAL_MIN -DBL_MAX+#define LM_REAL_EPSILON DBL_EPSILON+#define LM_CNST(x) (x)++#include "lm_core.c" // read in core code++#undef LM_REAL+#undef LM_PREFIX+#undef LM_REAL_MAX+#undef LM_REAL_EPSILON+#undef LM_REAL_MIN+#undef LM_CNST+#endif /* LM_DBL_PREC */
+ levmar-2.4/lm.h view
@@ -0,0 +1,282 @@+/*+////////////////////////////////////////////////////////////////////////////////////+//+// Prototypes and definitions for the Levenberg - Marquardt minimization algorithm+// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+////////////////////////////////////////////////////////////////////////////////////+*/++#ifndef _LM_H_+#define _LM_H_+++/************************************* Start of configuration options *************************************/++/* specify whether to use LAPACK or not. The first option is strongly recommended */+#define HAVE_LAPACK /* use LAPACK */+/* #undef HAVE_LAPACK */ /* uncomment this to force not using LAPACK */++/* to avoid the overhead of repeated mallocs(), routines in Axb.c can be instructed to+ * retain working memory between calls. Such a choice, however, renders these routines+ * non-reentrant and is not safe in a shared memory multiprocessing environment.+ * Bellow, this option is turned on only when not compiling with OpenMP.+ */+#if !defined(_OPENMP)+#define LINSOLVERS_RETAIN_MEMORY /* comment this if you don't want routines in Axb.c retain working memory between calls */+#endif++/* determine the precision variants to be build. Default settings build+ * both the single and double precision routines+ */+#define LM_DBL_PREC /* comment this if you don't want the double precision routines to be compiled */+#define LM_SNGL_PREC /* comment this if you don't want the single precision routines to be compiled */++/* Undefine the following if you don't want errors to be printed.*/+/* #define ENABLE_PRINT_ERROR */++/****************** End of configuration options, no changes necessary beyond this point ******************/+++#ifdef __cplusplus+extern "C" {+#endif++#ifdef ENABLE_PRINT_ERROR+ #define PRINT_ERROR(...) (fprintf(stderr, __VA_ARGS__))+#else+ #define PRINT_ERROR(...)+#endif++enum lmerror+{ LM_ERROR_LAPACK_ERROR = -1+, LM_ERROR_NO_JACOBIAN = -2+, LM_ERROR_NO_BOX_CONSTRAINTS = -3+, LM_ERROR_FAILED_BOX_CHECK = -4+, LM_ERROR_MEMORY_ALLOCATION_FAILURE = -5+, LM_ERROR_CONSTRAINT_MATRIX_ROWS_GT_COLS = -6+, LM_ERROR_CONSTRAINT_MATRIX_NOT_FULL_ROW_RANK = -7+, LM_ERROR_TOO_FEW_MEASUREMENTS = -8+, LM_ERROR_SINGULAR_MATRIX = -9+, LM_ERROR_SUM_OF_SQUARES_NOT_FINITE = -10+};++#define FABS(x) (((x)>=0.0)? (x) : -(x))++/* work arrays size for ?levmar_der and ?levmar_dif functions.+ * should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes+ */+#define LM_DER_WORKSZ(npar, nmeas) (2*(nmeas) + 4*(npar) + (nmeas)*(npar) + (npar)*(npar))+#define LM_DIF_WORKSZ(npar, nmeas) (4*(nmeas) + 4*(npar) + (nmeas)*(npar) + (npar)*(npar))++/* work arrays size for ?levmar_bc_der and ?levmar_bc_dif functions.+ * should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes+ */+#define LM_BC_DER_WORKSZ(npar, nmeas) (2*(nmeas) + 4*(npar) + (nmeas)*(npar) + (npar)*(npar))+#define LM_BC_DIF_WORKSZ(npar, nmeas) LM_BC_DER_WORKSZ((npar), (nmeas)) /* LEVMAR_BC_DIF currently implemented using LEVMAR_BC_DER()! */++/* work arrays size for ?levmar_lec_der and ?levmar_lec_dif functions.+ * should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes+ */+#define LM_LEC_DER_WORKSZ(npar, nmeas, nconstr) LM_DER_WORKSZ((npar)-(nconstr), (nmeas))+#define LM_LEC_DIF_WORKSZ(npar, nmeas, nconstr) LM_DIF_WORKSZ((npar)-(nconstr), (nmeas))++/* work arrays size for ?levmar_blec_der and ?levmar_blec_dif functions.+ * should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes+ */+#define LM_BLEC_DER_WORKSZ(npar, nmeas, nconstr) LM_LEC_DER_WORKSZ((npar), (nmeas)+(npar), (nconstr))+#define LM_BLEC_DIF_WORKSZ(npar, nmeas, nconstr) LM_LEC_DIF_WORKSZ((npar), (nmeas)+(npar), (nconstr))++#define LM_OPTS_SZ 5 /* max(4, 5) */+#define LM_INFO_SZ 10+#define LM_INIT_MU 1E-03+#define LM_STOP_THRESH 1E-17+#define LM_DIFF_DELTA 1E-06+#define LM_VERSION "2.4 (April 2009)"++#ifdef LM_DBL_PREC+/* double precision LM, with & without Jacobian */+/* unconstrained minimization */+extern int dlevmar_der(+ void (*func)(double *p, double *hx, int m, int n, void *adata),+ void (*jacf)(double *p, double *j, int m, int n, void *adata),+ double *p, double *x, int m, int n, int itmax, double *opts,+ double *info, double *work, double *covar, void *adata);++extern int dlevmar_dif(+ void (*func)(double *p, double *hx, int m, int n, void *adata),+ double *p, double *x, int m, int n, int itmax, double *opts,+ double *info, double *work, double *covar, void *adata);++/* box-constrained minimization */+extern int dlevmar_bc_der(+ void (*func)(double *p, double *hx, int m, int n, void *adata),+ void (*jacf)(double *p, double *j, int m, int n, void *adata),+ double *p, double *x, int m, int n, double *lb, double *ub,+ int itmax, double *opts, double *info, double *work, double *covar, void *adata);++extern int dlevmar_bc_dif(+ void (*func)(double *p, double *hx, int m, int n, void *adata),+ double *p, double *x, int m, int n, double *lb, double *ub,+ int itmax, double *opts, double *info, double *work, double *covar, void *adata);++#ifdef HAVE_LAPACK+/* linear equation constrained minimization */+extern int dlevmar_lec_der(+ void (*func)(double *p, double *hx, int m, int n, void *adata),+ void (*jacf)(double *p, double *j, int m, int n, void *adata),+ double *p, double *x, int m, int n, double *A, double *b, int k,+ int itmax, double *opts, double *info, double *work, double *covar, void *adata);++extern int dlevmar_lec_dif(+ void (*func)(double *p, double *hx, int m, int n, void *adata),+ double *p, double *x, int m, int n, double *A, double *b, int k,+ int itmax, double *opts, double *info, double *work, double *covar, void *adata);++/* box & linear equation constrained minimization */+extern int dlevmar_blec_der(+ void (*func)(double *p, double *hx, int m, int n, void *adata),+ void (*jacf)(double *p, double *j, int m, int n, void *adata),+ double *p, double *x, int m, int n, double *lb, double *ub, double *A, double *b, int k, double *wghts,+ int itmax, double *opts, double *info, double *work, double *covar, void *adata);++extern int dlevmar_blec_dif(+ void (*func)(double *p, double *hx, int m, int n, void *adata),+ double *p, double *x, int m, int n, double *lb, double *ub, double *A, double *b, int k, double *wghts,+ int itmax, double *opts, double *info, double *work, double *covar, void *adata);+#endif /* HAVE_LAPACK */++#endif /* LM_DBL_PREC */+++#ifdef LM_SNGL_PREC+/* single precision LM, with & without Jacobian */+/* unconstrained minimization */+extern int slevmar_der(+ void (*func)(float *p, float *hx, int m, int n, void *adata),+ void (*jacf)(float *p, float *j, int m, int n, void *adata),+ float *p, float *x, int m, int n, int itmax, float *opts,+ float *info, float *work, float *covar, void *adata);++extern int slevmar_dif(+ void (*func)(float *p, float *hx, int m, int n, void *adata),+ float *p, float *x, int m, int n, int itmax, float *opts,+ float *info, float *work, float *covar, void *adata);++/* box-constrained minimization */+extern int slevmar_bc_der(+ void (*func)(float *p, float *hx, int m, int n, void *adata),+ void (*jacf)(float *p, float *j, int m, int n, void *adata),+ float *p, float *x, int m, int n, float *lb, float *ub,+ int itmax, float *opts, float *info, float *work, float *covar, void *adata);++extern int slevmar_bc_dif(+ void (*func)(float *p, float *hx, int m, int n, void *adata),+ float *p, float *x, int m, int n, float *lb, float *ub,+ int itmax, float *opts, float *info, float *work, float *covar, void *adata);++#ifdef HAVE_LAPACK+/* linear equation constrained minimization */+extern int slevmar_lec_der(+ void (*func)(float *p, float *hx, int m, int n, void *adata),+ void (*jacf)(float *p, float *j, int m, int n, void *adata),+ float *p, float *x, int m, int n, float *A, float *b, int k,+ int itmax, float *opts, float *info, float *work, float *covar, void *adata);++extern int slevmar_lec_dif(+ void (*func)(float *p, float *hx, int m, int n, void *adata),+ float *p, float *x, int m, int n, float *A, float *b, int k,+ int itmax, float *opts, float *info, float *work, float *covar, void *adata);++/* box & linear equation constrained minimization */+extern int slevmar_blec_der(+ void (*func)(float *p, float *hx, int m, int n, void *adata),+ void (*jacf)(float *p, float *j, int m, int n, void *adata),+ float *p, float *x, int m, int n, float *lb, float *ub, float *A, float *b, int k, float *wghts,+ int itmax, float *opts, float *info, float *work, float *covar, void *adata);++extern int slevmar_blec_dif(+ void (*func)(float *p, float *hx, int m, int n, void *adata),+ float *p, float *x, int m, int n, float *lb, float *ub, float *A, float *b, int k, float *wghts,+ int itmax, float *opts, float *info, float *work, float *covar, void *adata);+#endif /* HAVE_LAPACK */++#endif /* LM_SNGL_PREC */++/* linear system solvers */+#ifdef HAVE_LAPACK++#ifdef LM_DBL_PREC+extern int dAx_eq_b_QR(double *A, double *B, double *x, int m);+extern int dAx_eq_b_QRLS(double *A, double *B, double *x, int m, int n);+extern int dAx_eq_b_Chol(double *A, double *B, double *x, int m);+extern int dAx_eq_b_LU(double *A, double *B, double *x, int m);+extern int dAx_eq_b_SVD(double *A, double *B, double *x, int m);+#endif /* LM_DBL_PREC */++#ifdef LM_SNGL_PREC+extern int sAx_eq_b_QR(float *A, float *B, float *x, int m);+extern int sAx_eq_b_QRLS(float *A, float *B, float *x, int m, int n);+extern int sAx_eq_b_Chol(float *A, float *B, float *x, int m);+extern int sAx_eq_b_LU(float *A, float *B, float *x, int m);+extern int sAx_eq_b_SVD(float *A, float *B, float *x, int m);+#endif /* LM_SNGL_PREC */++#else /* no LAPACK */++#ifdef LM_DBL_PREC+extern int dAx_eq_b_LU_noLapack(double *A, double *B, double *x, int n);+#endif /* LM_DBL_PREC */++#ifdef LM_SNGL_PREC+extern int sAx_eq_b_LU_noLapack(float *A, float *B, float *x, int n);+#endif /* LM_SNGL_PREC */++#endif /* HAVE_LAPACK */++/* Jacobian verification, double & single precision */+#ifdef LM_DBL_PREC+extern void dlevmar_chkjac(+ void (*func)(double *p, double *hx, int m, int n, void *adata),+ void (*jacf)(double *p, double *j, int m, int n, void *adata),+ double *p, int m, int n, void *adata, double *err);+#endif /* LM_DBL_PREC */++#ifdef LM_SNGL_PREC+extern void slevmar_chkjac(+ void (*func)(float *p, float *hx, int m, int n, void *adata),+ void (*jacf)(float *p, float *j, int m, int n, void *adata),+ float *p, int m, int n, void *adata, float *err);+#endif /* LM_SNGL_PREC */++/* standard deviation, coefficient of determination (R2) & Pearson's correlation coefficient for best-fit parameters */+#ifdef LM_DBL_PREC+extern double dlevmar_stddev( double *covar, int m, int i);+extern double dlevmar_corcoef(double *covar, int m, int i, int j);+extern double dlevmar_R2(void (*func)(double *p, double *hx, int m, int n, void *adata), double *p, double *x, int m, int n, void *adata);++#endif /* LM_DBL_PREC */++#ifdef LM_SNGL_PREC+extern float slevmar_stddev( float *covar, int m, int i);+extern float slevmar_corcoef(float *covar, int m, int i, int j);+extern float slevmar_R2(void (*func)(float *p, float *hx, int m, int n, void *adata), float *p, float *x, int m, int n, void *adata);+#endif /* LM_SNGL_PREC */++#ifdef __cplusplus+}+#endif++#endif /* _LM_H_ */
+ levmar-2.4/lm_core.c view
@@ -0,0 +1,847 @@+/////////////////////////////////////////////////////////////////////////////////+//+// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++#ifndef LM_REAL // not included by lm.c+#error This file should not be compiled directly!+#endif+++/* precision-specific definitions */+#define LEVMAR_DER LM_ADD_PREFIX(levmar_der)+#define LEVMAR_DIF LM_ADD_PREFIX(levmar_dif)+#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)+#define LEVMAR_FDIF_CENT_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_cent_jac_approx)+#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)+#define LEVMAR_L2NRMXMY LM_ADD_PREFIX(levmar_L2nrmxmy)+#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)++#ifdef HAVE_LAPACK+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU)+#define AX_EQ_B_CHOL LM_ADD_PREFIX(Ax_eq_b_Chol)+#define AX_EQ_B_QR LM_ADD_PREFIX(Ax_eq_b_QR)+#define AX_EQ_B_QRLS LM_ADD_PREFIX(Ax_eq_b_QRLS)+#define AX_EQ_B_SVD LM_ADD_PREFIX(Ax_eq_b_SVD)+#else+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU_noLapack)+#endif /* HAVE_LAPACK */++/*+ * This function seeks the parameter vector p that best describes the measurements vector x.+ * More precisely, given a vector function func : R^m --> R^n with n>=m,+ * it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of+ * e=x-func(p) is minimized.+ *+ * This function requires an analytic Jacobian. In case the latter is unavailable,+ * use LEVMAR_DIF() bellow+ *+ * Returns the number of iterations (>=0) if successful, or an error code (<0) on failure+ *+ * For more details, see K. Madsen, H.B. Nielsen and O. Tingleff's lecture notes on+ * non-linear least squares at http://www.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf+ */++int LEVMAR_DER(+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */+ void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), /* function to evaluate the Jacobian \part x / \part p */+ LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */+ LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */+ int m, /* I: parameter vector dimension (i.e. #unknowns) */+ int n, /* I: measurement vector dimension */+ int itmax, /* I: maximum number of iterations */+ LM_REAL opts[4], /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,+ * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used+ */+ LM_REAL info[LM_INFO_SZ],+ /* O: information regarding the minimization. Set to NULL if don't care+ * info[0]= ||e||_2 at initial p.+ * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.+ * info[5]= # iterations,+ * info[6]=reason for terminating: 1 - stopped by small gradient J^T e+ * 2 - stopped by small Dp+ * 3 - stopped by itmax+ * 4 - singular matrix. Restart from current p with increased mu+ * 5 - no further error reduction is possible. Restart with increased mu+ * 6 - stopped by small ||e||_2+ * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error+ * info[7]= # function evaluations+ * info[8]= # Jacobian evaluations+ * info[9]= # linear systems solved, i.e. # attempts for reducing error+ */+ LM_REAL *work, /* working memory at least LM_DER_WORKSZ() reals large, allocated if NULL */+ LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */+ void *adata) /* pointer to possibly additional data, passed uninterpreted to func & jacf.+ * Set to NULL if not needed+ */+{+register int i, j, k, l;+int worksz, freework=0, issolved;+/* temp work arrays */+LM_REAL *e, /* nx1 */+ *hx, /* \hat{x}_i, nx1 */+ *jacTe, /* J^T e_i mx1 */+ *jac, /* nxm */+ *jacTjac, /* mxm */+ *Dp, /* mx1 */+ *diag_jacTjac, /* diagonal of J^T J, mx1 */+ *pDp; /* p + Dp, mx1 */++register LM_REAL mu, /* damping constant */+ tmp; /* mainly used in matrix & vector multiplications */+LM_REAL p_eL2, jacTe_inf, pDp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+Dp)||_2 */+LM_REAL p_L2, Dp_L2=LM_REAL_MAX, dF, dL;+LM_REAL tau, eps1, eps2, eps2_sq, eps3;+LM_REAL init_p_eL2;+int nu=2, nu2, stop=0, nfev, njev=0, nlss=0;+const int nm=n*m;+int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;++ mu=jacTe_inf=0.0; /* -Wall */++ if(n<m){+ PRINT_ERROR(LCAT(LEVMAR_DER, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);+ return LM_ERROR_TOO_FEW_MEASUREMENTS;+ }++ if(!jacf){+ PRINT_ERROR(RCAT("No function specified for computing the Jacobian in ", LEVMAR_DER)+ RCAT("().\nIf no such function is available, use ", LEVMAR_DIF) RCAT("() rather than ", LEVMAR_DER) "()\n");+ return LM_ERROR_NO_JACOBIAN;+ }++ if(opts){+ tau=opts[0];+ eps1=opts[1];+ eps2=opts[2];+ eps2_sq=opts[2]*opts[2];+ eps3=opts[3];+ }+ else{ // use default values+ tau=LM_CNST(LM_INIT_MU);+ eps1=LM_CNST(LM_STOP_THRESH);+ eps2=LM_CNST(LM_STOP_THRESH);+ eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);+ eps3=LM_CNST(LM_STOP_THRESH);+ }++ if(!work){+ worksz=LM_DER_WORKSZ(m, n); //2*n+4*m + n*m + m*m;+ work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */+ if(!work){+ PRINT_ERROR(LCAT(LEVMAR_DER, "(): memory allocation request failed\n"));+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }+ freework=1;+ }++ /* set up work arrays */+ e=work;+ hx=e + n;+ jacTe=hx + n;+ jac=jacTe + m;+ jacTjac=jac + nm;+ Dp=jacTjac + m*m;+ diag_jacTjac=Dp + m;+ pDp=diag_jacTjac + m;++ /* compute e=x - f(p) and its L2 norm */+ (*func)(p, hx, m, n, adata); nfev=1;+ /* ### e=x-hx, p_eL2=||e|| */+#if 1+ p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);+#else+ for(i=0, p_eL2=0.0; i<n; ++i){+ e[i]=tmp=x[i]-hx[i];+ p_eL2+=tmp*tmp;+ }+#endif+ init_p_eL2=p_eL2;+ if(!LM_FINITE(p_eL2)) stop=7;++ for(k=0; k<itmax && !stop; ++k){+ /* Note that p and e have been updated at a previous iteration */++ if(p_eL2<=eps3){ /* error is small */+ stop=6;+ break;+ }++ /* Compute the Jacobian J at p, J^T J, J^T e, ||J^T e||_inf and ||p||^2.+ * Since J^T J is symmetric, its computation can be sped up by computing+ * only its upper triangular part and copying it to the lower part+ */++ (*jacf)(p, jac, m, n, adata); ++njev;++ /* J^T J, J^T e */+ if(nm<__BLOCKSZ__SQ){ // this is a small problem+ /* J^T*J_ij = \sum_l J^T_il * J_lj = \sum_l J_li * J_lj.+ * Thus, the product J^T J can be computed using an outer loop for+ * l that adds J_li*J_lj to each element ij of the result. Note that+ * with this scheme, the accesses to J and JtJ are always along rows,+ * therefore induces less cache misses compared to the straightforward+ * algorithm for computing the product (i.e., l loop is innermost one).+ * A similar scheme applies to the computation of J^T e.+ * However, for large minimization problems (i.e., involving a large number+ * of unknowns and measurements) for which J/J^T J rows are too large to+ * fit in the L1 cache, even this scheme incures many cache misses. In+ * such cases, a cache-efficient blocking scheme is preferable.+ *+ * Thanks to John Nitao of Lawrence Livermore Lab for pointing out this+ * performance problem.+ *+ * Note that the non-blocking algorithm is faster on small+ * problems since in this case it avoids the overheads of blocking.+ */++ /* looping downwards saves a few computations */+ register int l, im;+ register LM_REAL alpha, *jaclm;++ for(i=m*m; i-->0; )+ jacTjac[i]=0.0;+ for(i=m; i-->0; )+ jacTe[i]=0.0;++ for(l=n; l-->0; ){+ jaclm=jac+l*m;+ for(i=m; i-->0; ){+ im=i*m;+ alpha=jaclm[i]; //jac[l*m+i];+ for(j=i+1; j-->0; ) /* j<=i computes lower triangular part only */+ jacTjac[im+j]+=jaclm[j]*alpha; //jac[l*m+j]++ /* J^T e */+ jacTe[i]+=alpha*e[l];+ }+ }++ for(i=m; i-->0; ) /* copy to upper part */+ for(j=i+1; j<m; ++j)+ jacTjac[i*m+j]=jacTjac[j*m+i];++ }+ else{ // this is a large problem+ /* Cache efficient computation of J^T J based on blocking+ */+ LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);++ /* cache efficient computation of J^T e */+ for(i=0; i<m; ++i)+ jacTe[i]=0.0;++ for(i=0; i<n; ++i){+ register LM_REAL *jacrow;++ for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)+ jacTe[l]+=jacrow[l]*tmp;+ }+ }++ /* Compute ||J^T e||_inf and ||p||^2 */+ for(i=0, p_L2=jacTe_inf=0.0; i<m; ++i){+ if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;++ diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */+ p_L2+=p[i]*p[i];+ }+ //p_L2=sqrt(p_L2);++#if 0+if(!(k%100)){+ printf("Current estimate: ");+ for(i=0; i<m; ++i)+ printf("%.9g ", p[i]);+ printf("-- errors %.9g %0.9g\n", jacTe_inf, p_eL2);+}+#endif++ /* check for convergence */+ if((jacTe_inf <= eps1)){+ Dp_L2=0.0; /* no increment for p in this case */+ stop=1;+ break;+ }++ /* compute initial damping factor */+ if(k==0){+ for(i=0, tmp=LM_REAL_MIN; i<m; ++i)+ if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */+ mu=tau*tmp;+ }++ /* determine increment using adaptive damping */+ while(1){+ /* augment normal equations */+ for(i=0; i<m; ++i)+ jacTjac[i*m+i]+=mu;++ /* solve augmented equations */+#ifdef HAVE_LAPACK+ /* 5 alternatives are available: LU, Cholesky, 2 variants of QR decomposition and SVD.+ * Cholesky is the fastest but might be inaccurate; QR is slower but more accurate;+ * SVD is the slowest but most accurate; LU offers a tradeoff between accuracy and speed+ */++ issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;+ //issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_CHOL;+ //issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_QR;+ //issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); ++nlss; linsolver=(int (*)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m))AX_EQ_B_QRLS;+ //issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_SVD;++#else+ /* use the LU included with levmar */+ issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;+#endif /* HAVE_LAPACK */++ if(issolved){+ /* compute p's new estimate and ||Dp||^2 */+ for(i=0, Dp_L2=0.0; i<m; ++i){+ pDp[i]=p[i] + (tmp=Dp[i]);+ Dp_L2+=tmp*tmp;+ }+ //Dp_L2=sqrt(Dp_L2);++ if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */+ //if(Dp_L2<=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */+ stop=2;+ break;+ }++ if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */+ //if(Dp_L2>=(p_L2+eps2)/LM_CNST(EPSILON)){ /* almost singular */+ stop=4;+ break;+ }++ (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + Dp */+ /* compute ||e(pDp)||_2 */+ /* ### hx=x-hx, pDp_eL2=||hx|| */+#if 1+ pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);+#else+ for(i=0, pDp_eL2=0.0; i<n; ++i){+ hx[i]=tmp=x[i]-hx[i];+ pDp_eL2+=tmp*tmp;+ }+#endif+ if(!LM_FINITE(pDp_eL2)){ /* sum of squares is not finite, most probably due to a user error.+ * This check makes sure that the inner loop does not run indefinitely.+ * Thanks to Steve Danauskas for reporting such cases+ */+ stop=7;+ break;+ }++ for(i=0, dL=0.0; i<m; ++i)+ dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);++ dF=p_eL2-pDp_eL2;++ if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */+ tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));+ tmp=LM_CNST(1.0)-tmp*tmp*tmp;+ mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );+ nu=2;++ for(i=0 ; i<m; ++i) /* update p's estimate */+ p[i]=pDp[i];++ for(i=0; i<n; ++i) /* update e and ||e||_2 */+ e[i]=hx[i];+ p_eL2=pDp_eL2;+ break;+ }+ }++ /* if this point is reached, either the linear system could not be solved or+ * the error did not reduce; in any case, the increment must be rejected+ */++ mu*=nu;+ nu2=nu<<1; // 2*nu;+ if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */+ stop=5;+ break;+ }+ nu=nu2;++ for(i=0; i<m; ++i) /* restore diagonal J^T J entries */+ jacTjac[i*m+i]=diag_jacTjac[i];+ } /* inner loop */+ }++ if(k>=itmax) stop=3;++ for(i=0; i<m; ++i) /* restore diagonal J^T J entries */+ jacTjac[i*m+i]=diag_jacTjac[i];++ if(info){+ info[0]=init_p_eL2;+ info[1]=p_eL2;+ info[2]=jacTe_inf;+ info[3]=Dp_L2;+ for(i=0, tmp=LM_REAL_MIN; i<m; ++i)+ if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];+ info[4]=mu/tmp;+ info[5]=(LM_REAL)k;+ info[6]=(LM_REAL)stop;+ info[7]=(LM_REAL)nfev;+ info[8]=(LM_REAL)njev;+ info[9]=(LM_REAL)nlss;+ }++ /* covariance matrix */+ if(covar){+ LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);+ }++ if(freework) free(work);++#ifdef LINSOLVERS_RETAIN_MEMORY+ if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);+#endif++ switch (stop) {+ case 4: return LM_ERROR_SINGULAR_MATRIX;+ case 7: return LM_ERROR_SUM_OF_SQUARES_NOT_FINITE;+ default: return k;+ }+}+++/* Secant version of the LEVMAR_DER() function above: the Jacobian is approximated with+ * the aid of finite differences (forward or central, see the comment for the opts argument)+ */+int LEVMAR_DIF(+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */+ LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */+ LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */+ int m, /* I: parameter vector dimension (i.e. #unknowns) */+ int n, /* I: measurement vector dimension */+ int itmax, /* I: maximum number of iterations */+ LM_REAL opts[5], /* I: opts[0-4] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the+ * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and+ * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.+ * If \delta<0, the Jacobian is approximated with central differences which are more accurate+ * (but slower!) compared to the forward differences employed by default.+ */+ LM_REAL info[LM_INFO_SZ],+ /* O: information regarding the minimization. Set to NULL if don't care+ * info[0]= ||e||_2 at initial p.+ * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.+ * info[5]= # iterations,+ * info[6]=reason for terminating: 1 - stopped by small gradient J^T e+ * 2 - stopped by small Dp+ * 3 - stopped by itmax+ * 4 - singular matrix. Restart from current p with increased mu+ * 5 - no further error reduction is possible. Restart with increased mu+ * 6 - stopped by small ||e||_2+ * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error+ * info[7]= # function evaluations+ * info[8]= # Jacobian evaluations+ * info[9]= # linear systems solved, i.e. # attempts for reducing error+ */+ LM_REAL *work, /* working memory at least LM_DIF_WORKSZ() reals large, allocated if NULL */+ LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */+ void *adata) /* pointer to possibly additional data, passed uninterpreted to func.+ * Set to NULL if not needed+ */+{+register int i, j, k, l;+int worksz, freework=0, issolved;+/* temp work arrays */+LM_REAL *e, /* nx1 */+ *hx, /* \hat{x}_i, nx1 */+ *jacTe, /* J^T e_i mx1 */+ *jac, /* nxm */+ *jacTjac, /* mxm */+ *Dp, /* mx1 */+ *diag_jacTjac, /* diagonal of J^T J, mx1 */+ *pDp, /* p + Dp, mx1 */+ *wrk, /* nx1 */+ *wrk2; /* nx1, used only for holding a temporary e vector and when differentiating with central differences */++int using_ffdif=1;++register LM_REAL mu, /* damping constant */+ tmp; /* mainly used in matrix & vector multiplications */+LM_REAL p_eL2, jacTe_inf, pDp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+Dp)||_2 */+LM_REAL p_L2, Dp_L2=LM_REAL_MAX, dF, dL;+LM_REAL tau, eps1, eps2, eps2_sq, eps3, delta;+LM_REAL init_p_eL2;+int nu, nu2, stop=0, nfev, njap=0, nlss=0, K=(m>=10)? m: 10, updjac, updp=1, newjac;+const int nm=n*m;+int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;++ mu=jacTe_inf=p_L2=0.0; /* -Wall */+ updjac=newjac=0; /* -Wall */++ if(n<m){+ PRINT_ERROR(LCAT(LEVMAR_DIF, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);+ return LM_ERROR_TOO_FEW_MEASUREMENTS;+ }++ if(opts){+ tau=opts[0];+ eps1=opts[1];+ eps2=opts[2];+ eps2_sq=opts[2]*opts[2];+ eps3=opts[3];+ delta=opts[4];+ if(delta<0.0){+ delta=-delta; /* make positive */+ using_ffdif=0; /* use central differencing */+ }+ }+ else{ // use default values+ tau=LM_CNST(LM_INIT_MU);+ eps1=LM_CNST(LM_STOP_THRESH);+ eps2=LM_CNST(LM_STOP_THRESH);+ eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);+ eps3=LM_CNST(LM_STOP_THRESH);+ delta=LM_CNST(LM_DIFF_DELTA);+ }++ if(!work){+ worksz=LM_DIF_WORKSZ(m, n); //4*n+4*m + n*m + m*m;+ work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */+ if(!work){+ PRINT_ERROR(LCAT(LEVMAR_DIF, "(): memory allocation request failed\n"));+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }+ freework=1;+ }++ /* set up work arrays */+ e=work;+ hx=e + n;+ jacTe=hx + n;+ jac=jacTe + m;+ jacTjac=jac + nm;+ Dp=jacTjac + m*m;+ diag_jacTjac=Dp + m;+ pDp=diag_jacTjac + m;+ wrk=pDp + m;+ wrk2=wrk + n;++ /* compute e=x - f(p) and its L2 norm */+ (*func)(p, hx, m, n, adata); nfev=1;+ /* ### e=x-hx, p_eL2=||e|| */+#if 1+ p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);+#else+ for(i=0, p_eL2=0.0; i<n; ++i){+ e[i]=tmp=x[i]-hx[i];+ p_eL2+=tmp*tmp;+ }+#endif+ init_p_eL2=p_eL2;+ if(!LM_FINITE(p_eL2)) stop=7;++ nu=20; /* force computation of J */++ for(k=0; k<itmax && !stop; ++k){+ /* Note that p and e have been updated at a previous iteration */++ if(p_eL2<=eps3){ /* error is small */+ stop=6;+ break;+ }++ /* Compute the Jacobian J at p, J^T J, J^T e, ||J^T e||_inf and ||p||^2.+ * The symmetry of J^T J is again exploited for speed+ */++ if((updp && nu>16) || updjac==K){ /* compute difference approximation to J */+ if(using_ffdif){ /* use forward differences */+ LEVMAR_FDIF_FORW_JAC_APPROX(func, p, hx, wrk, delta, jac, m, n, adata);+ ++njap; nfev+=m;+ }+ else{ /* use central differences */+ LEVMAR_FDIF_CENT_JAC_APPROX(func, p, wrk, wrk2, delta, jac, m, n, adata);+ ++njap; nfev+=2*m;+ }+ nu=2; updjac=0; updp=0; newjac=1;+ }++ if(newjac){ /* Jacobian has changed, recompute J^T J, J^t e, etc */+ newjac=0;++ /* J^T J, J^T e */+ if(nm<=__BLOCKSZ__SQ){ // this is a small problem+ /* J^T*J_ij = \sum_l J^T_il * J_lj = \sum_l J_li * J_lj.+ * Thus, the product J^T J can be computed using an outer loop for+ * l that adds J_li*J_lj to each element ij of the result. Note that+ * with this scheme, the accesses to J and JtJ are always along rows,+ * therefore induces less cache misses compared to the straightforward+ * algorithm for computing the product (i.e., l loop is innermost one).+ * A similar scheme applies to the computation of J^T e.+ * However, for large minimization problems (i.e., involving a large number+ * of unknowns and measurements) for which J/J^T J rows are too large to+ * fit in the L1 cache, even this scheme incures many cache misses. In+ * such cases, a cache-efficient blocking scheme is preferable.+ *+ * Thanks to John Nitao of Lawrence Livermore Lab for pointing out this+ * performance problem.+ *+ * Note that the non-blocking algorithm is faster on small+ * problems since in this case it avoids the overheads of blocking.+ */+ register int l, im;+ register LM_REAL alpha, *jaclm;++ /* looping downwards saves a few computations */+ for(i=m*m; i-->0; )+ jacTjac[i]=0.0;+ for(i=m; i-->0; )+ jacTe[i]=0.0;++ for(l=n; l-->0; ){+ jaclm=jac+l*m;+ for(i=m; i-->0; ){+ im=i*m;+ alpha=jaclm[i]; //jac[l*m+i];+ for(j=i+1; j-->0; ) /* j<=i computes lower triangular part only */+ jacTjac[im+j]+=jaclm[j]*alpha; //jac[l*m+j]++ /* J^T e */+ jacTe[i]+=alpha*e[l];+ }+ }++ for(i=m; i-->0; ) /* copy to upper part */+ for(j=i+1; j<m; ++j)+ jacTjac[i*m+j]=jacTjac[j*m+i];+ }+ else{ // this is a large problem+ /* Cache efficient computation of J^T J based on blocking+ */+ LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);++ /* cache efficient computation of J^T e */+ for(i=0; i<m; ++i)+ jacTe[i]=0.0;++ for(i=0; i<n; ++i){+ register LM_REAL *jacrow;++ for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)+ jacTe[l]+=jacrow[l]*tmp;+ }+ }++ /* Compute ||J^T e||_inf and ||p||^2 */+ for(i=0, p_L2=jacTe_inf=0.0; i<m; ++i){+ if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;++ diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */+ p_L2+=p[i]*p[i];+ }+ //p_L2=sqrt(p_L2);+ }++#if 0+if(!(k%100)){+ printf("Current estimate: ");+ for(i=0; i<m; ++i)+ printf("%.9g ", p[i]);+ printf("-- errors %.9g %0.9g\n", jacTe_inf, p_eL2);+}+#endif++ /* check for convergence */+ if((jacTe_inf <= eps1)){+ Dp_L2=0.0; /* no increment for p in this case */+ stop=1;+ break;+ }++ /* compute initial damping factor */+ if(k==0){+ for(i=0, tmp=LM_REAL_MIN; i<m; ++i)+ if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */+ mu=tau*tmp;+ }++ /* determine increment using adaptive damping */++ /* augment normal equations */+ for(i=0; i<m; ++i)+ jacTjac[i*m+i]+=mu;++ /* solve augmented equations */+#ifdef HAVE_LAPACK+ /* 5 alternatives are available: LU, Cholesky, 2 variants of QR decomposition and SVD.+ * Cholesky is the fastest but might be inaccurate; QR is slower but more accurate;+ * SVD is the slowest but most accurate; LU offers a tradeoff between accuracy and speed+ */++ issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;+ //issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_CHOL;+ //issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_QR;+ //issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); ++nlss; linsolver=(int (*)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m))AX_EQ_B_QRLS;+ //issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_SVD;+#else+ /* use the LU included with levmar */+ issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;+#endif /* HAVE_LAPACK */++ if(issolved){+ /* compute p's new estimate and ||Dp||^2 */+ for(i=0, Dp_L2=0.0; i<m; ++i){+ pDp[i]=p[i] + (tmp=Dp[i]);+ Dp_L2+=tmp*tmp;+ }+ //Dp_L2=sqrt(Dp_L2);++ if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */+ //if(Dp_L2<=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */+ stop=2;+ break;+ }++ if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */+ //if(Dp_L2>=(p_L2+eps2)/LM_CNST(EPSILON)){ /* almost singular */+ stop=4;+ break;+ }++ (*func)(pDp, wrk, m, n, adata); ++nfev; /* evaluate function at p + Dp */+ /* compute ||e(pDp)||_2 */+ /* ### wrk2=x-wrk, pDp_eL2=||wrk2|| */+#if 1+ pDp_eL2=LEVMAR_L2NRMXMY(wrk2, x, wrk, n);+#else+ for(i=0, pDp_eL2=0.0; i<n; ++i){+ wrk2[i]=tmp=x[i]-wrk[i];+ pDp_eL2+=tmp*tmp;+ }+#endif+ if(!LM_FINITE(pDp_eL2)){ /* sum of squares is not finite, most probably due to a user error.+ * This check makes sure that the loop terminates early in the case+ * of invalid input. Thanks to Steve Danauskas for suggesting it+ */++ stop=7;+ break;+ }++ dF=p_eL2-pDp_eL2;+ if(updp || dF>0){ /* update jac */+ for(i=0; i<n; ++i){+ for(l=0, tmp=0.0; l<m; ++l)+ tmp+=jac[i*m+l]*Dp[l]; /* (J * Dp)[i] */+ tmp=(wrk[i] - hx[i] - tmp)/Dp_L2; /* (f(p+dp)[i] - f(p)[i] - (J * Dp)[i])/(dp^T*dp) */+ for(j=0; j<m; ++j)+ jac[i*m+j]+=tmp*Dp[j];+ }+ ++updjac;+ newjac=1;+ }++ for(i=0, dL=0.0; i<m; ++i)+ dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);++ if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */+ tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));+ tmp=LM_CNST(1.0)-tmp*tmp*tmp;+ mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );+ nu=2;++ for(i=0 ; i<m; ++i) /* update p's estimate */+ p[i]=pDp[i];++ for(i=0; i<n; ++i){ /* update e, hx and ||e||_2 */+ e[i]=wrk2[i]; //x[i]-wrk[i];+ hx[i]=wrk[i];+ }+ p_eL2=pDp_eL2;+ updp=1;+ continue;+ }+ }++ /* if this point is reached, either the linear system could not be solved or+ * the error did not reduce; in any case, the increment must be rejected+ */++ mu*=nu;+ nu2=nu<<1; // 2*nu;+ if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */+ stop=5;+ break;+ }+ nu=nu2;++ for(i=0; i<m; ++i) /* restore diagonal J^T J entries */+ jacTjac[i*m+i]=diag_jacTjac[i];+ }++ if(k>=itmax) stop=3;++ for(i=0; i<m; ++i) /* restore diagonal J^T J entries */+ jacTjac[i*m+i]=diag_jacTjac[i];++ if(info){+ info[0]=init_p_eL2;+ info[1]=p_eL2;+ info[2]=jacTe_inf;+ info[3]=Dp_L2;+ for(i=0, tmp=LM_REAL_MIN; i<m; ++i)+ if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];+ info[4]=mu/tmp;+ info[5]=(LM_REAL)k;+ info[6]=(LM_REAL)stop;+ info[7]=(LM_REAL)nfev;+ info[8]=(LM_REAL)njap;+ info[9]=(LM_REAL)nlss;+ }++ /* covariance matrix */+ if(covar){+ LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);+ }+++ if(freework) free(work);++#ifdef LINSOLVERS_RETAIN_MEMORY+ if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);+#endif++ switch (stop) {+ case 4: return LM_ERROR_SINGULAR_MATRIX;+ case 7: return LM_ERROR_SUM_OF_SQUARES_NOT_FINITE;+ default: return k;+ }+}++/* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */+#undef LEVMAR_DER+#undef LEVMAR_DIF+#undef LEVMAR_FDIF_FORW_JAC_APPROX+#undef LEVMAR_FDIF_CENT_JAC_APPROX+#undef LEVMAR_COVAR+#undef LEVMAR_TRANS_MAT_MAT_MULT+#undef LEVMAR_L2NRMXMY+#undef AX_EQ_B_LU+#undef AX_EQ_B_CHOL+#undef AX_EQ_B_QR+#undef AX_EQ_B_QRLS+#undef AX_EQ_B_SVD
+ levmar-2.4/lmbc.c view
@@ -0,0 +1,85 @@+/////////////////////////////////////////////////////////////////////////////////+// +// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++/******************************************************************************** + * Box-constrained Levenberg-Marquardt nonlinear minimization. The same core code+ * is used with appropriate #defines to derive single and double precision versions,+ * see also lmbc_core.c+ ********************************************************************************/++#include <stdio.h>+#include <stdlib.h>+#include <math.h>+#include <float.h>++#include "lm.h"+#include "compiler.h"+#include "misc.h"++#define EPSILON 1E-12+#define ONE_THIRD 0.3333333334 /* 1.0/3.0 */++#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!+#endif+++#ifdef LM_SNGL_PREC+/* single precision (float) definitions */+#define LM_REAL float+#define LM_PREFIX s++#define LM_REAL_MAX FLT_MAX+#define LM_REAL_MIN -FLT_MAX++#define LM_REAL_EPSILON FLT_EPSILON+#define __SUBCNST(x) x##F+#define LM_CNST(x) __SUBCNST(x) // force substitution++#include "lmbc_core.c" // read in core code++#undef LM_REAL+#undef LM_PREFIX+#undef LM_REAL_MAX+#undef LM_REAL_MIN+#undef LM_REAL_EPSILON+#undef __SUBCNST+#undef LM_CNST+#endif /* LM_SNGL_PREC */++#ifdef LM_DBL_PREC+/* double precision definitions */+#define LM_REAL double+#define LM_PREFIX d++#define LM_REAL_MAX DBL_MAX+#define LM_REAL_MIN -DBL_MAX++#define LM_REAL_EPSILON DBL_EPSILON+#define LM_CNST(x) (x)++#include "lmbc_core.c" // read in core code++#undef LM_REAL+#undef LM_PREFIX+#undef LM_REAL_MAX+#undef LM_REAL_MIN+#undef LM_REAL_EPSILON+#undef LM_CNST+#endif /* LM_DBL_PREC */
+ levmar-2.4/lmbc_core.c view
@@ -0,0 +1,949 @@+/////////////////////////////////////////////////////////////////////////////////+//+// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++#ifndef LM_REAL // not included by lmbc.c+#error This file should not be compiled directly!+#endif+++/* precision-specific definitions */+#define FUNC_STATE LM_ADD_PREFIX(func_state)+#define LNSRCH LM_ADD_PREFIX(lnsrch)+#define BOXPROJECT LM_ADD_PREFIX(boxProject)+#define LEVMAR_BOX_CHECK LM_ADD_PREFIX(levmar_box_check)+#define LEVMAR_BC_DER LM_ADD_PREFIX(levmar_bc_der)+#define LEVMAR_BC_DIF LM_ADD_PREFIX(levmar_bc_dif)+#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)+#define LEVMAR_FDIF_CENT_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_cent_jac_approx)+#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)+#define LEVMAR_L2NRMXMY LM_ADD_PREFIX(levmar_L2nrmxmy)+#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)+#define LMBC_DIF_DATA LM_ADD_PREFIX(lmbc_dif_data)+#define LMBC_DIF_FUNC LM_ADD_PREFIX(lmbc_dif_func)+#define LMBC_DIF_JACF LM_ADD_PREFIX(lmbc_dif_jacf)++#ifdef HAVE_LAPACK+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU)+#define AX_EQ_B_CHOL LM_ADD_PREFIX(Ax_eq_b_Chol)+#define AX_EQ_B_QR LM_ADD_PREFIX(Ax_eq_b_QR)+#define AX_EQ_B_QRLS LM_ADD_PREFIX(Ax_eq_b_QRLS)+#define AX_EQ_B_SVD LM_ADD_PREFIX(Ax_eq_b_SVD)+#else+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU_noLapack)+#endif /* HAVE_LAPACK */++/* find the median of 3 numbers */+#define __MEDIAN3(a, b, c) ( ((a) >= (b))?\+ ( ((c) >= (a))? (a) : ( ((c) <= (b))? (b) : (c) ) ) : \+ ( ((c) >= (b))? (b) : ( ((c) <= (a))? (a) : (c) ) ) )++#define _POW_ LM_CNST(2.1)++#define __LSITMAX 150 // max #iterations for line search++struct FUNC_STATE{+ int n, *nfev;+ LM_REAL *hx, *x;+ void *adata;+};++static void+LNSRCH(int m, LM_REAL *x, LM_REAL f, LM_REAL *g, LM_REAL *p, LM_REAL alpha, LM_REAL *xpls,+ LM_REAL *ffpls, void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), struct FUNC_STATE state,+ int *mxtake, int *iretcd, LM_REAL stepmx, LM_REAL steptl, LM_REAL *sx)+{+/* Find a next newton iterate by backtracking line search.+ * Specifically, finds a \lambda such that for a fixed alpha<0.5 (usually 1e-4),+ * f(x + \lambda*p) <= f(x) + alpha * \lambda * g^T*p+ *+ * Translated (with minor changes) from Schnabel, Koontz & Weiss uncmin.f, v1.3++ * PARAMETERS :++ * m --> dimension of problem (i.e. number of variables)+ * x(m) --> old iterate: x[k-1]+ * f --> function value at old iterate, f(x)+ * g(m) --> gradient at old iterate, g(x), or approximate+ * p(m) --> non-zero newton step+ * alpha --> fixed constant < 0.5 for line search (see above)+ * xpls(m) <-- new iterate x[k]+ * ffpls <-- function value at new iterate, f(xpls)+ * func --> name of subroutine to evaluate function+ * state <--> information other than x and m that func requires.+ * state is not modified in xlnsrch (but can be modified by func).+ * iretcd <-- return code+ * mxtake <-- boolean flag indicating step of maximum length used+ * stepmx --> maximum allowable step size+ * steptl --> relative step size at which successive iterates+ * considered close enough to terminate algorithm+ * sx(m) --> diagonal scaling matrix for x, can be NULL++ * internal variables++ * sln newton length+ * rln relative length of newton step+*/++ register int i, j;+ int firstback = 1;+ LM_REAL disc;+ LM_REAL a3, b;+ LM_REAL t1, t2, t3, lambda, tlmbda, rmnlmb;+ LM_REAL scl, rln, sln, slp;+ LM_REAL tmp1, tmp2;+ LM_REAL fpls, pfpls = 0., plmbda = 0.; /* -Wall */++ f*=LM_CNST(0.5);+ *mxtake = 0;+ *iretcd = 2;+ tmp1 = 0.;+ if(!sx) /* no scaling */+ for (i = 0; i < m; ++i)+ tmp1 += p[i] * p[i];+ else+ for (i = 0; i < m; ++i)+ tmp1 += sx[i] * sx[i] * p[i] * p[i];+ sln = (LM_REAL)sqrt(tmp1);+ if (sln > stepmx) {+ /* newton step longer than maximum allowed */+ scl = stepmx / sln;+ for(i=0; i<m; ++i) /* p * scl */+ p[i]*=scl;+ sln = stepmx;+ }+ for(i=0, slp=0.; i<m; ++i) /* g^T * p */+ slp+=g[i]*p[i];+ rln = 0.;+ if(!sx) /* no scaling */+ for (i = 0; i < m; ++i) {+ tmp1 = (FABS(x[i])>=LM_CNST(1.))? FABS(x[i]) : LM_CNST(1.);+ tmp2 = FABS(p[i])/tmp1;+ if(rln < tmp2) rln = tmp2;+ }+ else+ for (i = 0; i < m; ++i) {+ tmp1 = (FABS(x[i])>=LM_CNST(1.)/sx[i])? FABS(x[i]) : LM_CNST(1.)/sx[i];+ tmp2 = FABS(p[i])/tmp1;+ if(rln < tmp2) rln = tmp2;+ }+ rmnlmb = steptl / rln;+ lambda = LM_CNST(1.0);++ /* check if new iterate satisfactory. generate new lambda if necessary. */++ for(j=__LSITMAX; j>=0; --j) {+ for (i = 0; i < m; ++i)+ xpls[i] = x[i] + lambda * p[i];++ /* evaluate function at new point */+ (*func)(xpls, state.hx, m, state.n, state.adata); ++(*(state.nfev));+ /* ### state.hx=state.x-state.hx, tmp1=||state.hx|| */+#if 1+ tmp1=LEVMAR_L2NRMXMY(state.hx, state.x, state.hx, state.n);+#else+ for(i=0, tmp1=0.0; i<state.n; ++i){+ state.hx[i]=tmp2=state.x[i]-state.hx[i];+ tmp1+=tmp2*tmp2;+ }+#endif+ fpls=LM_CNST(0.5)*tmp1; *ffpls=tmp1;++ if (fpls <= f + slp * alpha * lambda) { /* solution found */+ *iretcd = 0;+ if (lambda == LM_CNST(1.) && sln > stepmx * LM_CNST(.99)) *mxtake = 1;+ return;+ }++ /* else : solution not (yet) found */++ /* First find a point with a finite value */++ if (lambda < rmnlmb) {+ /* no satisfactory xpls found sufficiently distinct from x */++ *iretcd = 1;+ return;+ }+ else { /* calculate new lambda */++ /* modifications to cover non-finite values */+ if (!LM_FINITE(fpls)) {+ lambda *= LM_CNST(0.1);+ firstback = 1;+ }+ else {+ if (firstback) { /* first backtrack: quadratic fit */+ tlmbda = -lambda * slp / ((fpls - f - slp) * LM_CNST(2.));+ firstback = 0;+ }+ else { /* all subsequent backtracks: cubic fit */+ t1 = fpls - f - lambda * slp;+ t2 = pfpls - f - plmbda * slp;+ t3 = LM_CNST(1.) / (lambda - plmbda);+ a3 = LM_CNST(3.) * t3 * (t1 / (lambda * lambda)+ - t2 / (plmbda * plmbda));+ b = t3 * (t2 * lambda / (plmbda * plmbda)+ - t1 * plmbda / (lambda * lambda));+ disc = b * b - a3 * slp;+ if (disc > b * b)+ /* only one positive critical point, must be minimum */+ tlmbda = (-b + ((a3 < 0)? -(LM_REAL)sqrt(disc): (LM_REAL)sqrt(disc))) /a3;+ else+ /* both critical points positive, first is minimum */+ tlmbda = (-b + ((a3 < 0)? (LM_REAL)sqrt(disc): -(LM_REAL)sqrt(disc))) /a3;++ if (tlmbda > lambda * LM_CNST(.5))+ tlmbda = lambda * LM_CNST(.5);+ }+ plmbda = lambda;+ pfpls = fpls;+ if (tlmbda < lambda * LM_CNST(.1))+ lambda *= LM_CNST(.1);+ else+ lambda = tlmbda;+ }+ }+ }+ /* this point is reached when the iterations limit is exceeded */+ *iretcd = 1; /* failed */+ return;+} /* LNSRCH */++/* Projections to feasible set \Omega: P_{\Omega}(y) := arg min { ||x - y|| : x \in \Omega}, y \in R^m */++/* project vector p to a box shaped feasible set. p is a mx1 vector.+ * Either lb, ub can be NULL. If not NULL, they are mx1 vectors+ */+static void BOXPROJECT(LM_REAL *p, LM_REAL *lb, LM_REAL *ub, int m)+{+register int i;++ if(!lb){ /* no lower bounds */+ if(!ub) /* no upper bounds */+ return;+ else{ /* upper bounds only */+ for(i=0; i<m; ++i)+ if(p[i]>ub[i]) p[i]=ub[i];+ }+ }+ else+ if(!ub){ /* lower bounds only */+ for(i=0; i<m; ++i)+ if(p[i]<lb[i]) p[i]=lb[i];+ }+ else /* box bounds */+ for(i=0; i<m; ++i)+ p[i]=__MEDIAN3(lb[i], p[i], ub[i]);+}++/*+ * This function seeks the parameter vector p that best describes the measurements+ * vector x under box constraints.+ * More precisely, given a vector function func : R^m --> R^n with n>=m,+ * it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of+ * e=x-func(p) is minimized under the constraints lb[i]<=p[i]<=ub[i].+ * If no lower bound constraint applies for p[i], use -DBL_MAX/-FLT_MAX for lb[i];+ * If no upper bound constraint applies for p[i], use DBL_MAX/FLT_MAX for ub[i].+ *+ * This function requires an analytic Jacobian. In case the latter is unavailable,+ * use LEVMAR_BC_DIF() bellow+ *+ * Returns the number of iterations (>=0) if successful, or an error code (<0) on failure.+ *+ * For details, see C. Kanzow, N. Yamashita and M. Fukushima: "Levenberg-Marquardt+ * methods for constrained nonlinear equations with strong local convergence properties",+ * Journal of Computational and Applied Mathematics 172, 2004, pp. 375-397.+ * Also, see K. Madsen, H.B. Nielsen and O. Tingleff's lecture notes on+ * unconstrained Levenberg-Marquardt at http://www.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf+ */++int LEVMAR_BC_DER(+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */+ void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), /* function to evaluate the Jacobian \part x / \part p */+ LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */+ LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */+ int m, /* I: parameter vector dimension (i.e. #unknowns) */+ int n, /* I: measurement vector dimension */+ LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */+ LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */+ int itmax, /* I: maximum number of iterations */+ LM_REAL opts[4], /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,+ * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used.+ * Note that ||J^T e||_inf is computed on free (not equal to lb[i] or ub[i]) variables only.+ */+ LM_REAL info[LM_INFO_SZ],+ /* O: information regarding the minimization. Set to NULL if don't care+ * info[0]= ||e||_2 at initial p.+ * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.+ * info[5]= # iterations,+ * info[6]=reason for terminating: 1 - stopped by small gradient J^T e+ * 2 - stopped by small Dp+ * 3 - stopped by itmax+ * 4 - singular matrix. Restart from current p with increased mu+ * 5 - no further error reduction is possible. Restart with increased mu+ * 6 - stopped by small ||e||_2+ * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error+ * info[7]= # function evaluations+ * info[8]= # Jacobian evaluations+ * info[9]= # linear systems solved, i.e. # attempts for reducing error+ */+ LM_REAL *work, /* working memory at least LM_BC_DER_WORKSZ() reals large, allocated if NULL */+ LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */+ void *adata) /* pointer to possibly additional data, passed uninterpreted to func & jacf.+ * Set to NULL if not needed+ */+{+register int i, j, k, l;+int worksz, freework=0, issolved;+/* temp work arrays */+LM_REAL *e, /* nx1 */+ *hx, /* \hat{x}_i, nx1 */+ *jacTe, /* J^T e_i mx1 */+ *jac, /* nxm */+ *jacTjac, /* mxm */+ *Dp, /* mx1 */+ *diag_jacTjac, /* diagonal of J^T J, mx1 */+ *pDp; /* p + Dp, mx1 */++register LM_REAL mu, /* damping constant */+ tmp; /* mainly used in matrix & vector multiplications */+LM_REAL p_eL2, jacTe_inf, pDp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+Dp)||_2 */+LM_REAL p_L2, Dp_L2=LM_REAL_MAX, dF, dL;+LM_REAL tau, eps1, eps2, eps2_sq, eps3;+LM_REAL init_p_eL2;+int nu=2, nu2, stop=0, nfev, njev=0, nlss=0;+const int nm=n*m;++/* variables for constrained LM */+struct FUNC_STATE fstate;+LM_REAL alpha=LM_CNST(1e-4), beta=LM_CNST(0.9), gamma=LM_CNST(0.99995), gamma_sq=gamma*gamma, rho=LM_CNST(1e-8);+LM_REAL t, t0;+LM_REAL steptl=LM_CNST(1e3)*(LM_REAL)sqrt(LM_REAL_EPSILON), jacTeDp;+LM_REAL tmin=LM_CNST(1e-12), tming=LM_CNST(1e-18); /* minimum step length for LS and PG steps */+const LM_REAL tini=LM_CNST(1.0); /* initial step length for LS and PG steps */+int nLMsteps=0, nLSsteps=0, nPGsteps=0, gprevtaken=0;+int numactive;+int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;++ mu=jacTe_inf=t=0.0; tmin=tmin; /* -Wall */++ if(n<m){+ PRINT_ERROR(LCAT(LEVMAR_BC_DER, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);+ return LM_ERROR_TOO_FEW_MEASUREMENTS;+ }++ if(!jacf){+ PRINT_ERROR(RCAT("No function specified for computing the Jacobian in ", LEVMAR_BC_DER)+ RCAT("().\nIf no such function is available, use ", LEVMAR_BC_DIF) RCAT("() rather than ", LEVMAR_BC_DER) "()\n");+ return LM_ERROR_NO_JACOBIAN;+ }++ if(!LEVMAR_BOX_CHECK(lb, ub, m)){+ PRINT_ERROR(LCAT(LEVMAR_BC_DER, "(): at least one lower bound exceeds the upper one\n"));+ return LM_ERROR_FAILED_BOX_CHECK;+ }++ if(opts){+ tau=opts[0];+ eps1=opts[1];+ eps2=opts[2];+ eps2_sq=opts[2]*opts[2];+ eps3=opts[3];+ }+ else{ // use default values+ tau=LM_CNST(LM_INIT_MU);+ eps1=LM_CNST(LM_STOP_THRESH);+ eps2=LM_CNST(LM_STOP_THRESH);+ eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);+ eps3=LM_CNST(LM_STOP_THRESH);+ }++ if(!work){+ worksz=LM_BC_DER_WORKSZ(m, n); //2*n+4*m + n*m + m*m;+ work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */+ if(!work){+ PRINT_ERROR(LCAT(LEVMAR_BC_DER, "(): memory allocation request failed\n"));+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }+ freework=1;+ }++ /* set up work arrays */+ e=work;+ hx=e + n;+ jacTe=hx + n;+ jac=jacTe + m;+ jacTjac=jac + nm;+ Dp=jacTjac + m*m;+ diag_jacTjac=Dp + m;+ pDp=diag_jacTjac + m;++ fstate.n=n;+ fstate.hx=hx;+ fstate.x=x;+ fstate.adata=adata;+ fstate.nfev=&nfev;++ /* see if starting point is within the feasile set */+ for(i=0; i<m; ++i)+ pDp[i]=p[i];+ BOXPROJECT(p, lb, ub, m); /* project to feasible set */+ for(i=0; i<m; ++i)+ if(pDp[i]!=p[i])+ PRINT_ERROR(RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_BC_DER) "()! [%g projected to %g]\n",+ i, pDp[i], p[i]);++ /* compute e=x - f(p) and its L2 norm */+ (*func)(p, hx, m, n, adata); nfev=1;+ /* ### e=x-hx, p_eL2=||e|| */+#if 1+ p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);+#else+ for(i=0, p_eL2=0.0; i<n; ++i){+ e[i]=tmp=x[i]-hx[i];+ p_eL2+=tmp*tmp;+ }+#endif+ init_p_eL2=p_eL2;+ if(!LM_FINITE(p_eL2)) stop=7;++ for(k=0; k<itmax && !stop; ++k){+ /* Note that p and e have been updated at a previous iteration */++ if(p_eL2<=eps3){ /* error is small */+ stop=6;+ break;+ }++ /* Compute the Jacobian J at p, J^T J, J^T e, ||J^T e||_inf and ||p||^2.+ * Since J^T J is symmetric, its computation can be sped up by computing+ * only its upper triangular part and copying it to the lower part+ */++ (*jacf)(p, jac, m, n, adata); ++njev;++ /* J^T J, J^T e */+ if(nm<__BLOCKSZ__SQ){ // this is a small problem+ /* J^T*J_ij = \sum_l J^T_il * J_lj = \sum_l J_li * J_lj.+ * Thus, the product J^T J can be computed using an outer loop for+ * l that adds J_li*J_lj to each element ij of the result. Note that+ * with this scheme, the accesses to J and JtJ are always along rows,+ * therefore induces less cache misses compared to the straightforward+ * algorithm for computing the product (i.e., l loop is innermost one).+ * A similar scheme applies to the computation of J^T e.+ * However, for large minimization problems (i.e., involving a large number+ * of unknowns and measurements) for which J/J^T J rows are too large to+ * fit in the L1 cache, even this scheme incures many cache misses. In+ * such cases, a cache-efficient blocking scheme is preferable.+ *+ * Thanks to John Nitao of Lawrence Livermore Lab for pointing out this+ * performance problem.+ *+ * Note that the non-blocking algorithm is faster on small+ * problems since in this case it avoids the overheads of blocking.+ */+ register int l, im;+ register LM_REAL alpha, *jaclm;++ /* looping downwards saves a few computations */+ for(i=m*m; i-->0; )+ jacTjac[i]=0.0;+ for(i=m; i-->0; )+ jacTe[i]=0.0;++ for(l=n; l-->0; ){+ jaclm=jac+l*m;+ for(i=m; i-->0; ){+ im=i*m;+ alpha=jaclm[i]; //jac[l*m+i];+ for(j=i+1; j-->0; ) /* j<=i computes lower triangular part only */+ jacTjac[im+j]+=jaclm[j]*alpha; //jac[l*m+j]++ /* J^T e */+ jacTe[i]+=alpha*e[l];+ }+ }++ for(i=m; i-->0; ) /* copy to upper part */+ for(j=i+1; j<m; ++j)+ jacTjac[i*m+j]=jacTjac[j*m+i];+ }+ else{ // this is a large problem+ /* Cache efficient computation of J^T J based on blocking+ */+ LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);++ /* cache efficient computation of J^T e */+ for(i=0; i<m; ++i)+ jacTe[i]=0.0;++ for(i=0; i<n; ++i){+ register LM_REAL *jacrow;++ for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)+ jacTe[l]+=jacrow[l]*tmp;+ }+ }++ /* Compute ||J^T e||_inf and ||p||^2. Note that ||J^T e||_inf+ * is computed for free (i.e. inactive) variables only.+ * At a local minimum, if p[i]==ub[i] then g[i]>0;+ * if p[i]==lb[i] g[i]<0; otherwise g[i]=0+ */+ for(i=j=numactive=0, p_L2=jacTe_inf=0.0; i<m; ++i){+ if(ub && p[i]==ub[i]){ ++numactive; if(jacTe[i]>0.0) ++j; }+ else if(lb && p[i]==lb[i]){ ++numactive; if(jacTe[i]<0.0) ++j; }+ else if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;++ diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */+ p_L2+=p[i]*p[i];+ }+ //p_L2=sqrt(p_L2);++#if 0+if(!(k%100)){+ printf("Current estimate: ");+ for(i=0; i<m; ++i)+ printf("%.9g ", p[i]);+ printf("-- errors %.9g %0.9g, #active %d [%d]\n", jacTe_inf, p_eL2, numactive, j);+}+#endif++ /* check for convergence */+ if(j==numactive && (jacTe_inf <= eps1)){+ Dp_L2=0.0; /* no increment for p in this case */+ stop=1;+ break;+ }++ /* compute initial damping factor */+ if(k==0){+ if(!lb && !ub){ /* no bounds */+ for(i=0, tmp=LM_REAL_MIN; i<m; ++i)+ if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */+ mu=tau*tmp;+ }+ else+ mu=LM_CNST(0.5)*tau*p_eL2; /* use Kanzow's starting mu */+ }++ /* determine increment using a combination of adaptive damping, line search and projected gradient search */+ while(1){+ /* augment normal equations */+ for(i=0; i<m; ++i)+ jacTjac[i*m+i]+=mu;++ /* solve augmented equations */+#ifdef HAVE_LAPACK+ /* 5 alternatives are available: LU, Cholesky, 2 variants of QR decomposition and SVD.+ * Cholesky is the fastest but might be inaccurate; QR is slower but more accurate;+ * SVD is the slowest but most accurate; LU offers a tradeoff between accuracy and speed+ */++ issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;+ //issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_CHOL;+ //issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_QR;+ //issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); ++nlss; linsolver=(int (*)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m))AX_EQ_B_QRLS;+ //issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_SVD;++#else+ /* use the LU included with levmar */+ issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;+#endif /* HAVE_LAPACK */++ if(issolved){+ for(i=0; i<m; ++i)+ pDp[i]=p[i] + Dp[i];++ /* compute p's new estimate and ||Dp||^2 */+ BOXPROJECT(pDp, lb, ub, m); /* project to feasible set */+ for(i=0, Dp_L2=0.0; i<m; ++i){+ Dp[i]=tmp=pDp[i]-p[i];+ Dp_L2+=tmp*tmp;+ }+ //Dp_L2=sqrt(Dp_L2);++ if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */+ stop=2;+ break;+ }++ if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */+ stop=4;+ break;+ }++ (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + Dp */+ /* ### hx=x-hx, pDp_eL2=||hx|| */+#if 1+ pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);+#else+ for(i=0, pDp_eL2=0.0; i<n; ++i){ /* compute ||e(pDp)||_2 */+ hx[i]=tmp=x[i]-hx[i];+ pDp_eL2+=tmp*tmp;+ }+#endif+ if(!LM_FINITE(pDp_eL2)){+ stop=7;+ break;+ }++ if(pDp_eL2<=gamma_sq*p_eL2){+ for(i=0, dL=0.0; i<m; ++i)+ dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);++#if 1+ if(dL>0.0){+ dF=p_eL2-pDp_eL2;+ tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));+ tmp=LM_CNST(1.0)-tmp*tmp*tmp;+ mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );+ }+ else+ mu=(mu>=pDp_eL2)? pDp_eL2 : mu; /* pDp_eL2 is the new pDp_eL2 */+#else++ mu=(mu>=pDp_eL2)? pDp_eL2 : mu; /* pDp_eL2 is the new pDp_eL2 */+#endif++ nu=2;++ for(i=0 ; i<m; ++i) /* update p's estimate */+ p[i]=pDp[i];++ for(i=0; i<n; ++i) /* update e and ||e||_2 */+ e[i]=hx[i];+ p_eL2=pDp_eL2;+ ++nLMsteps;+ gprevtaken=0;+ break;+ }+ }+ else{++ /* the augmented linear system could not be solved, increase mu */++ mu*=nu;+ nu2=nu<<1; // 2*nu;+ if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */+ stop=5;+ break;+ }+ nu=nu2;++ for(i=0; i<m; ++i) /* restore diagonal J^T J entries */+ jacTjac[i*m+i]=diag_jacTjac[i];++ continue; /* solve again with increased nu */+ }++ /* if this point is reached, the LM step did not reduce the error;+ * see if it is a descent direction+ */++ /* negate jacTe (i.e. g) & compute g^T * Dp */+ for(i=0, jacTeDp=0.0; i<m; ++i){+ jacTe[i]=-jacTe[i];+ jacTeDp+=jacTe[i]*Dp[i];+ }++ if(jacTeDp<=-rho*pow(Dp_L2, _POW_/LM_CNST(2.0))){+ /* Dp is a descent direction; do a line search along it */+ int mxtake, iretcd;+ LM_REAL stepmx;++ tmp=(LM_REAL)sqrt(p_L2); stepmx=LM_CNST(1e3)*( (tmp>=LM_CNST(1.0))? tmp : LM_CNST(1.0) );++#if 1+ /* use Schnabel's backtracking line search; it requires fewer "func" evaluations */+ LNSRCH(m, p, p_eL2, jacTe, Dp, alpha, pDp, &pDp_eL2, func, fstate,+ &mxtake, &iretcd, stepmx, steptl, NULL); /* NOTE: LNSRCH() updates hx */+ if(iretcd!=0) goto gradproj; /* rather inelegant but effective way to handle LNSRCH() failures... */+#else+ /* use the simpler (but slower!) line search described by Kanzow et al */+ for(t=tini; t>tmin; t*=beta){+ for(i=0; i<m; ++i){+ pDp[i]=p[i] + t*Dp[i];+ //pDp[i]=__MEDIAN3(lb[i], pDp[i], ub[i]); /* project to feasible set */+ }++ (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + t*Dp */+ for(i=0, pDp_eL2=0.0; i<n; ++i){ /* compute ||e(pDp)||_2 */+ hx[i]=tmp=x[i]-hx[i];+ pDp_eL2+=tmp*tmp;+ }+ if(!LM_FINITE(pDp_eL2)) goto gradproj; /* treat as line search failure */++ //if(LM_CNST(0.5)*pDp_eL2<=LM_CNST(0.5)*p_eL2 + t*alpha*jacTeDp) break;+ if(pDp_eL2<=p_eL2 + LM_CNST(2.0)*t*alpha*jacTeDp) break;+ }+#endif+ ++nLSsteps;+ gprevtaken=0;++ /* NOTE: new estimate for p is in pDp, associated error in hx and its norm in pDp_eL2.+ * These values are used below to update their corresponding variables+ */+ }+ else{+gradproj: /* Note that this point can also be reached via a goto when LNSRCH() fails */++ /* jacTe is a descent direction; make a projected gradient step */++ /* if the previous step was along the gradient descent, try to use the t employed in that step */+ /* compute ||g|| */+ for(i=0, tmp=0.0; i<m; ++i)+ tmp+=jacTe[i]*jacTe[i];+ tmp=(LM_REAL)sqrt(tmp);+ tmp=LM_CNST(100.0)/(LM_CNST(1.0)+tmp);+ t0=(tmp<=tini)? tmp : tini; /* guard against poor scaling & large steps; see (3.50) in C.T. Kelley's book */++ for(t=(gprevtaken)? t : t0; t>tming; t*=beta){+ for(i=0; i<m; ++i)+ pDp[i]=p[i] - t*jacTe[i];+ BOXPROJECT(pDp, lb, ub, m); /* project to feasible set */+ for(i=0; i<m; ++i)+ Dp[i]=pDp[i]-p[i];++ (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p - t*g */+ /* compute ||e(pDp)||_2 */+ /* ### hx=x-hx, pDp_eL2=||hx|| */+#if 1+ pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);+#else+ for(i=0, pDp_eL2=0.0; i<n; ++i){+ hx[i]=tmp=x[i]-hx[i];+ pDp_eL2+=tmp*tmp;+ }+#endif+ if(!LM_FINITE(pDp_eL2)){+ stop=7;+ goto breaknested;+ }++ for(i=0, tmp=0.0; i<m; ++i) /* compute ||g^T * Dp|| */+ tmp+=jacTe[i]*Dp[i];++ if(gprevtaken && pDp_eL2<=p_eL2 + LM_CNST(2.0)*LM_CNST(0.99999)*tmp){ /* starting t too small */+ t=t0;+ gprevtaken=0;+ continue;+ }+ //if(LM_CNST(0.5)*pDp_eL2<=LM_CNST(0.5)*p_eL2 + alpha*tmp) break;+ if(pDp_eL2<=p_eL2 + LM_CNST(2.0)*alpha*tmp) break;+ }++ ++nPGsteps;+ gprevtaken=1;+ /* NOTE: new estimate for p is in pDp, associated error in hx and its norm in pDp_eL2 */+ }++ /* update using computed values */++ for(i=0, Dp_L2=0.0; i<m; ++i){+ tmp=pDp[i]-p[i];+ Dp_L2+=tmp*tmp;+ }+ //Dp_L2=sqrt(Dp_L2);++ if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */+ stop=2;+ break;+ }++ for(i=0 ; i<m; ++i) /* update p's estimate */+ p[i]=pDp[i];++ for(i=0; i<n; ++i) /* update e and ||e||_2 */+ e[i]=hx[i];+ p_eL2=pDp_eL2;+ break;+ } /* inner loop */+ }++breaknested: /* NOTE: this point is also reached via an explicit goto! */++ if(k>=itmax) stop=3;++ for(i=0; i<m; ++i) /* restore diagonal J^T J entries */+ jacTjac[i*m+i]=diag_jacTjac[i];++ if(info){+ info[0]=init_p_eL2;+ info[1]=p_eL2;+ info[2]=jacTe_inf;+ info[3]=Dp_L2;+ for(i=0, tmp=LM_REAL_MIN; i<m; ++i)+ if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];+ info[4]=mu/tmp;+ info[5]=(LM_REAL)k;+ info[6]=(LM_REAL)stop;+ info[7]=(LM_REAL)nfev;+ info[8]=(LM_REAL)njev;+ info[9]=(LM_REAL)nlss;+ }++ /* covariance matrix */+ if(covar){+ LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);+ }++ if(freework) free(work);++#ifdef LINSOLVERS_RETAIN_MEMORY+ if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);+#endif++#if 0+printf("%d LM steps, %d line search, %d projected gradient\n", nLMsteps, nLSsteps, nPGsteps);+#endif++ switch (stop) {+ case 4: return LM_ERROR_SINGULAR_MATRIX;+ case 7: return LM_ERROR_SUM_OF_SQUARES_NOT_FINITE;+ default: return k;+ }+}++/* following struct & LMBC_DIF_XXX functions won't be necessary if a true secant+ * version of LEVMAR_BC_DIF() is implemented...+ */+struct LMBC_DIF_DATA{+ int ffdif; // nonzero if forward differencing is used+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);+ LM_REAL *hx, *hxx;+ void *adata;+ LM_REAL delta;+};++static void LMBC_DIF_FUNC(LM_REAL *p, LM_REAL *hx, int m, int n, void *data)+{+struct LMBC_DIF_DATA *dta=(struct LMBC_DIF_DATA *)data;++ /* call user-supplied function passing it the user-supplied data */+ (*(dta->func))(p, hx, m, n, dta->adata);+}++static void LMBC_DIF_JACF(LM_REAL *p, LM_REAL *jac, int m, int n, void *data)+{+struct LMBC_DIF_DATA *dta=(struct LMBC_DIF_DATA *)data;++ if(dta->ffdif){+ /* evaluate user-supplied function at p */+ (*(dta->func))(p, dta->hx, m, n, dta->adata);+ LEVMAR_FDIF_FORW_JAC_APPROX(dta->func, p, dta->hx, dta->hxx, dta->delta, jac, m, n, dta->adata);+ }+ else+ LEVMAR_FDIF_CENT_JAC_APPROX(dta->func, p, dta->hx, dta->hxx, dta->delta, jac, m, n, dta->adata);+}+++/* No Jacobian version of the LEVMAR_BC_DER() function above: the Jacobian is approximated with+ * the aid of finite differences (forward or central, see the comment for the opts argument)+ * Ideally, this function should be implemented with a secant approach. Currently, it just calls+ * LEVMAR_BC_DER()+ */+int LEVMAR_BC_DIF(+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */+ LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */+ LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */+ int m, /* I: parameter vector dimension (i.e. #unknowns) */+ int n, /* I: measurement vector dimension */+ LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */+ LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */+ int itmax, /* I: maximum number of iterations */+ LM_REAL opts[5], /* I: opts[0-4] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the+ * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and+ * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.+ * If \delta<0, the Jacobian is approximated with central differences which are more accurate+ * (but slower!) compared to the forward differences employed by default.+ */+ LM_REAL info[LM_INFO_SZ],+ /* O: information regarding the minimization. Set to NULL if don't care+ * info[0]= ||e||_2 at initial p.+ * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.+ * info[5]= # iterations,+ * info[6]=reason for terminating: 1 - stopped by small gradient J^T e+ * 2 - stopped by small Dp+ * 3 - stopped by itmax+ * 4 - singular matrix. Restart from current p with increased mu+ * 5 - no further error reduction is possible. Restart with increased mu+ * 6 - stopped by small ||e||_2+ * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error+ * info[7]= # function evaluations+ * info[8]= # Jacobian evaluations+ * info[9]= # linear systems solved, i.e. # attempts for reducing error+ */+ LM_REAL *work, /* working memory at least LM_BC_DIF_WORKSZ() reals large, allocated if NULL */+ LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */+ void *adata) /* pointer to possibly additional data, passed uninterpreted to func.+ * Set to NULL if not needed+ */+{+struct LMBC_DIF_DATA data;+int ret;++ //PRINT_ERROR(RCAT("\nWarning: current implementation of ", LEVMAR_BC_DIF) "() does not use a secant approach!\n\n");++ data.ffdif=!opts || opts[4]>=0.0;++ data.func=func;+ data.hx=(LM_REAL *)malloc(2*n*sizeof(LM_REAL)); /* allocate a big chunk in one step */+ if(!data.hx){+ PRINT_ERROR(LCAT(LEVMAR_BC_DIF, "(): memory allocation request failed\n"));+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }+ data.hxx=data.hx+n;+ data.adata=adata;+ data.delta=(opts)? FABS(opts[4]) : (LM_REAL)LM_DIFF_DELTA;++ ret=LEVMAR_BC_DER(LMBC_DIF_FUNC, LMBC_DIF_JACF, p, x, m, n, lb, ub, itmax, opts, info, work, covar, (void *)&data);++ if(info){ /* correct the number of function calls */+ if(data.ffdif)+ info[7]+=info[8]*(m+1); /* each Jacobian evaluation costs m+1 function calls */+ else+ info[7]+=info[8]*(2*m); /* each Jacobian evaluation costs 2*m function calls */+ }++ free(data.hx);++ return ret;+}++/* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */+#undef FUNC_STATE+#undef LNSRCH+#undef BOXPROJECT+#undef LEVMAR_BOX_CHECK+#undef LEVMAR_BC_DER+#undef LMBC_DIF_DATA+#undef LMBC_DIF_FUNC+#undef LMBC_DIF_JACF+#undef LEVMAR_BC_DIF+#undef LEVMAR_FDIF_FORW_JAC_APPROX+#undef LEVMAR_FDIF_CENT_JAC_APPROX+#undef LEVMAR_COVAR+#undef LEVMAR_TRANS_MAT_MAT_MULT+#undef LEVMAR_L2NRMXMY+#undef AX_EQ_B_LU+#undef AX_EQ_B_CHOL+#undef AX_EQ_B_QR+#undef AX_EQ_B_QRLS+#undef AX_EQ_B_SVD
+ levmar-2.4/lmblec.c view
@@ -0,0 +1,87 @@+/////////////////////////////////////////////////////////////////////////////////+// +// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004-06 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++/******************************************************************************** + * combined box and linear equation constraints Levenberg-Marquardt nonlinear+ * minimization. The same core code is used with appropriate #defines to derive+ * single and double precision versions, see also lmblec_core.c+ ********************************************************************************/++#include <stdio.h>+#include <stdlib.h>+#include <math.h>+#include <float.h>++#include "lm.h"+#include "misc.h"++#ifndef HAVE_LAPACK++#ifdef _MSC_VER+#pragma message("Combined box and linearly constrained optimization requires LAPACK and was not compiled!")+#else+#warning Combined box and linearly constrained optimization requires LAPACK and was not compiled!+#endif // _MSC_VER++#else // LAPACK present++#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!+#endif+++#ifdef LM_SNGL_PREC+/* single precision (float) definitions */+#define LM_REAL float+#define LM_PREFIX s++#define LM_REAL_MAX FLT_MAX+#define LM_REAL_MIN -FLT_MAX+#define __SUBCNST(x) x##F+#define LM_CNST(x) __SUBCNST(x) // force substitution++#include "lmblec_core.c" // read in core code++#undef LM_REAL+#undef LM_PREFIX+#undef LM_REAL_MAX+#undef LM_REAL_MIN+#undef __SUBCNST+#undef LM_CNST+#endif /* LM_SNGL_PREC */++#ifdef LM_DBL_PREC+/* double precision definitions */+#define LM_REAL double+#define LM_PREFIX d++#define LM_REAL_MAX DBL_MAX+#define LM_REAL_MIN -DBL_MAX+#define LM_CNST(x) (x)++#include "lmblec_core.c" // read in core code++#undef LM_REAL+#undef LM_PREFIX+#undef LM_REAL_MAX+#undef LM_REAL_MIN+#undef LM_CNST+#endif /* LM_DBL_PREC */++#endif /* HAVE_LAPACK */
+ levmar-2.4/lmblec_core.c view
@@ -0,0 +1,413 @@+/////////////////////////////////////////////////////////////////////////////////+//+// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004-06 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++/*******************************************************************************+ * This file implements combined box and linear equation constraints.+ *+ * Note that the algorithm implementing linearly constrained minimization does+ * so by a change in parameters that transforms the original program into an+ * unconstrained one. To employ the same idea for implementing box & linear+ * constraints would require the transformation of box constraints on the+ * original parameters to box constraints for the new parameter set. This+ * being impossible, a different approach is used here for finding the minimum.+ * The trick is to remove the box constraints by augmenting the function to+ * be fitted with penalty terms and then solve the resulting problem (which+ * involves linear constrains only) with the functions in lmlec.c+ *+ * More specifically, for the constraint a<=x[i]<=b to hold, the term C[i]=+ * (2*x[i]-(a+b))/(b-a) should be within [-1, 1]. This is enforced by adding+ * the penalty term w[i]*max((C[i])^2-1, 0) to the objective function, where+ * w[i] is a large weight. In the case of constraints of the form a<=x[i],+ * the term C[i]=a-x[i] has to be non positive, thus the penalty term is+ * w[i]*max(C[i], 0). If x[i]<=b, C[i]=x[i]-b has to be non negative and+ * the penalty is w[i]*max(C[i], 0). The derivatives needed for the Jacobian+ * are as follows:+ * For the constraint a<=x[i]<=b: 4*(2*x[i]-(a+b))/(b-a)^2 if x[i] not in [a, b],+ * 0 otherwise+ * For the constraint a<=x[i]: -1 if x[i]<=a, 0 otherwise+ * For the constraint x[i]<=b: 1 if b<=x[i], 0 otherwise+ *+ * Note that for the above to work, the weights w[i] should be large enough;+ * depending on your minimization problem, the default values might need some+ * tweaking (see arg "wghts" below).+ *******************************************************************************/++#ifndef LM_REAL // not included by lmblec.c+#error This file should not be compiled directly!+#endif+++#define __MAX__(x, y) (((x)>=(y))? (x) : (y))+#define __BC_WEIGHT__ LM_CNST(1E+04)++#define __BC_INTERVAL__ 0+#define __BC_LOW__ 1+#define __BC_HIGH__ 2++/* precision-specific definitions */+#define LEVMAR_BOX_CHECK LM_ADD_PREFIX(levmar_box_check)+#define LMBLEC_DATA LM_ADD_PREFIX(lmblec_data)+#define LMBLEC_FUNC LM_ADD_PREFIX(lmblec_func)+#define LMBLEC_JACF LM_ADD_PREFIX(lmblec_jacf)+#define LEVMAR_LEC_DER LM_ADD_PREFIX(levmar_lec_der)+#define LEVMAR_LEC_DIF LM_ADD_PREFIX(levmar_lec_dif)+#define LEVMAR_BLEC_DER LM_ADD_PREFIX(levmar_blec_der)+#define LEVMAR_BLEC_DIF LM_ADD_PREFIX(levmar_blec_dif)+#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)++struct LMBLEC_DATA{+ LM_REAL *x, *lb, *ub, *w;+ int *bctype;+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);+ void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);+ void *adata;+};++/* augmented measurements */+static void LMBLEC_FUNC(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata)+{+struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;+int nn;+register int i, j, *typ;+register LM_REAL *lb, *ub, *w, tmp;++ nn=n-m;+ lb=data->lb;+ ub=data->ub;+ w=data->w;+ typ=data->bctype;+ (*(data->func))(p, hx, m, nn, data->adata);++ for(i=nn, j=0; i<n; ++i, ++j){+ switch(typ[j]){+ case __BC_INTERVAL__:+ tmp=(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(ub[j]-lb[j]);+ hx[i]=w[j]*__MAX__(tmp*tmp-LM_CNST(1.0), LM_CNST(0.0));+ break;++ case __BC_LOW__:+ hx[i]=w[j]*__MAX__(lb[j]-p[j], LM_CNST(0.0));+ break;++ case __BC_HIGH__:+ hx[i]=w[j]*__MAX__(p[j]-ub[j], LM_CNST(0.0));+ break;+ }+ }+}++/* augmented Jacobian */+static void LMBLEC_JACF(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata)+{+struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;+int nn, *typ;+register int i, j;+register LM_REAL *lb, *ub, *w, tmp;++ nn=n-m;+ lb=data->lb;+ ub=data->ub;+ w=data->w;+ typ=data->bctype;+ (*(data->jacf))(p, jac, m, nn, data->adata);++ /* clear all extra rows */+ for(i=nn*m; i<n*m; ++i)+ jac[i]=0.0;++ for(i=nn, j=0; i<n; ++i, ++j){+ switch(typ[j]){+ case __BC_INTERVAL__:+ if(lb[j]<=p[j] && p[j]<=ub[j])+ continue; // corresp. jac element already 0++ /* out of interval */+ tmp=ub[j]-lb[j];+ tmp=LM_CNST(4.0)*(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(tmp*tmp);+ jac[i*m+j]=w[j]*tmp;+ break;++ case __BC_LOW__: // (lb[j]<=p[j])? 0.0 : -1.0;+ if(lb[j]<=p[j])+ continue; // corresp. jac element already 0++ /* smaller than lower bound */+ jac[i*m+j]=-w[j];+ break;++ case __BC_HIGH__: // (p[j]<=ub[j])? 0.0 : 1.0;+ if(p[j]<=ub[j])+ continue; // corresp. jac element already 0++ /* greater than upper bound */+ jac[i*m+j]=w[j];+ break;+ }+ }+}++/*+ * This function seeks the parameter vector p that best describes the measurements+ * vector x under box & linear constraints.+ * More precisely, given a vector function func : R^m --> R^n with n>=m,+ * it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of+ * e=x-func(p) is minimized under the constraints lb[i]<=p[i]<=ub[i] and A p=b;+ * A is kxm, b kx1. Note that this function DOES NOT check the satisfiability of+ * the specified box and linear equation constraints.+ * If no lower bound constraint applies for p[i], use -DBL_MAX/-FLT_MAX for lb[i];+ * If no upper bound constraint applies for p[i], use DBL_MAX/FLT_MAX for ub[i].+ *+ * This function requires an analytic Jacobian. In case the latter is unavailable,+ * use LEVMAR_BLEC_DIF() bellow+ *+ * Returns the number of iterations (>=0) if successful, or an error code (<0) on failure.+ *+ * For more details on the algorithm implemented by this function, please refer to+ * the comments in the top of this file.+ *+ */+int LEVMAR_BLEC_DER(+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */+ void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), /* function to evaluate the Jacobian \part x / \part p */+ LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */+ LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */+ int m, /* I: parameter vector dimension (i.e. #unknowns) */+ int n, /* I: measurement vector dimension */+ LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */+ LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */+ LM_REAL *A, /* I: constraints matrix, kxm */+ LM_REAL *b, /* I: right hand constraints vector, kx1 */+ int k, /* I: number of constraints (i.e. A's #rows) */+ LM_REAL *wghts, /* mx1 weights for penalty terms, defaults used if NULL */+ int itmax, /* I: maximum number of iterations */+ LM_REAL opts[4], /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,+ * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used+ */+ LM_REAL info[LM_INFO_SZ],+ /* O: information regarding the minimization. Set to NULL if don't care+ * info[0]= ||e||_2 at initial p.+ * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.+ * info[5]= # iterations,+ * info[6]=reason for terminating: 1 - stopped by small gradient J^T e+ * 2 - stopped by small Dp+ * 3 - stopped by itmax+ * 4 - singular matrix. Restart from current p with increased mu+ * 5 - no further error reduction is possible. Restart with increased mu+ * 6 - stopped by small ||e||_2+ * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error+ * info[7]= # function evaluations+ * info[8]= # Jacobian evaluations+ * info[9]= # linear systems solved, i.e. # attempts for reducing error+ */+ LM_REAL *work, /* working memory at least LM_BLEC_DER_WORKSZ() reals large, allocated if NULL */+ LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */+ void *adata) /* pointer to possibly additional data, passed uninterpreted to func & jacf.+ * Set to NULL if not needed+ */+{+ struct LMBLEC_DATA data;+ int ret;+ LM_REAL locinfo[LM_INFO_SZ];+ register int i;++ if(!jacf){+ PRINT_ERROR(RCAT("No function specified for computing the Jacobian in ", LEVMAR_BLEC_DER)+ RCAT("().\nIf no such function is available, use ", LEVMAR_BLEC_DIF) RCAT("() rather than ", LEVMAR_BLEC_DER) "()\n");+ return LM_ERROR_NO_JACOBIAN;+ }++ if(!lb && !ub){+ PRINT_ERROR(RCAT(LCAT(LEVMAR_BLEC_DER, "(): lower and upper bounds for box constraints cannot be both NULL, use "),+ LEVMAR_LEC_DER) "() in this case!\n");+ return LM_ERROR_NO_BOX_CONSTRAINTS;+ }++ if(!LEVMAR_BOX_CHECK(lb, ub, m)){+ PRINT_ERROR(LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));+ return LM_ERROR_FAILED_BOX_CHECK;+ }++ /* measurement vector needs to be extended by m */+ if(x){ /* nonzero x */+ data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));+ if(!data.x){+ PRINT_ERROR(LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }++ for(i=0; i<n; ++i)+ data.x[i]=x[i];+ for(i=n; i<n+m; ++i)+ data.x[i]=0.0;+ }+ else+ data.x=NULL;++ data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int)); /* should be arranged in that order for proper doubles alignment */+ if(!data.w){+ PRINT_ERROR(LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));+ if(data.x) free(data.x);+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }+ data.bctype=(int *)(data.w+m);++ /* note: at this point, one of lb, ub are not NULL */+ for(i=0; i<m; ++i){+ data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];+ if(!lb) data.bctype[i]=__BC_HIGH__;+ else if(!ub) data.bctype[i]=__BC_LOW__;+ else if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;+ else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;+ else data.bctype[i]=__BC_HIGH__;+ }++ data.lb=lb;+ data.ub=ub;+ data.func=func;+ data.jacf=jacf;+ data.adata=adata;++ if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DER() is called with non-null info */+ ret=LEVMAR_LEC_DER(LMBLEC_FUNC, LMBLEC_JACF, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);++ if(data.x) free(data.x);+ free(data.w);++ return ret;+}++/* Similar to the LEVMAR_BLEC_DER() function above, except that the Jacobian is approximated+ * with the aid of finite differences (forward or central, see the comment for the opts argument)+ */+int LEVMAR_BLEC_DIF(+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */+ LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */+ LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */+ int m, /* I: parameter vector dimension (i.e. #unknowns) */+ int n, /* I: measurement vector dimension */+ LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */+ LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */+ LM_REAL *A, /* I: constraints matrix, kxm */+ LM_REAL *b, /* I: right hand constraints vector, kx1 */+ int k, /* I: number of constraints (i.e. A's #rows) */+ LM_REAL *wghts, /* mx1 weights for penalty terms, defaults used if NULL */+ int itmax, /* I: maximum number of iterations */+ LM_REAL opts[5], /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the+ * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and+ * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.+ * If \delta<0, the Jacobian is approximated with central differences which are more accurate+ * (but slower!) compared to the forward differences employed by default.+ */+ LM_REAL info[LM_INFO_SZ],+ /* O: information regarding the minimization. Set to NULL if don't care+ * info[0]= ||e||_2 at initial p.+ * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.+ * info[5]= # iterations,+ * info[6]=reason for terminating: 1 - stopped by small gradient J^T e+ * 2 - stopped by small Dp+ * 3 - stopped by itmax+ * 4 - singular matrix. Restart from current p with increased mu+ * 5 - no further error reduction is possible. Restart with increased mu+ * 6 - stopped by small ||e||_2+ * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error+ * info[7]= # function evaluations+ * info[8]= # Jacobian evaluations+ * info[9]= # linear systems solved, i.e. # attempts for reducing error+ */+ LM_REAL *work, /* working memory at least LM_BLEC_DIF_WORKSZ() reals large, allocated if NULL */+ LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */+ void *adata) /* pointer to possibly additional data, passed uninterpreted to func.+ * Set to NULL if not needed+ */+{+ struct LMBLEC_DATA data;+ int ret;+ register int i;+ LM_REAL locinfo[LM_INFO_SZ];++ if(!lb && !ub){+ PRINT_ERROR(RCAT(LCAT(LEVMAR_BLEC_DIF, "(): lower and upper bounds for box constraints cannot be both NULL, use "),+ LEVMAR_LEC_DIF) "() in this case!\n");+ return LM_ERROR_NO_BOX_CONSTRAINTS;+ }++ if(!LEVMAR_BOX_CHECK(lb, ub, m)){+ PRINT_ERROR(LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));+ return LM_ERROR_FAILED_BOX_CHECK;+ }++ /* measurement vector needs to be extended by m */+ if(x){ /* nonzero x */+ data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));+ if(!data.x){+ PRINT_ERROR(LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }++ for(i=0; i<n; ++i)+ data.x[i]=x[i];+ for(i=n; i<n+m; ++i)+ data.x[i]=0.0;+ }+ else+ data.x=NULL;++ data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int)); /* should be arranged in that order for proper doubles alignment */+ if(!data.w){+ PRINT_ERROR(LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));+ if(data.x) free(data.x);+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }+ data.bctype=(int *)(data.w+m);++ /* note: at this point, one of lb, ub are not NULL */+ for(i=0; i<m; ++i){+ data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];+ if(!lb) data.bctype[i]=__BC_HIGH__;+ else if(!ub) data.bctype[i]=__BC_LOW__;+ else if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;+ else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;+ else data.bctype[i]=__BC_HIGH__;+ }++ data.lb=lb;+ data.ub=ub;+ data.func=func;+ data.jacf=NULL;+ data.adata=adata;++ if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DIF() is called with non-null info */+ ret=LEVMAR_LEC_DIF(LMBLEC_FUNC, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);++ if(data.x) free(data.x);+ free(data.w);++ return ret;+}++/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */+#undef LEVMAR_BOX_CHECK+#undef LMBLEC_DATA+#undef LMBLEC_FUNC+#undef LMBLEC_JACF+#undef LEVMAR_COVAR+#undef LEVMAR_LEC_DER+#undef LEVMAR_LEC_DIF+#undef LEVMAR_BLEC_DER+#undef LEVMAR_BLEC_DIF
+ levmar-2.4/lmdemo.c view
@@ -0,0 +1,1028 @@+/////////////////////////////////////////////////////////////////////////////////+//+// Demonstration driver program for the Levenberg - Marquardt minimization+// algorithm+// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++/********************************************************************************+ * Levenberg-Marquardt minimization demo driver. Only the double precision versions+ * are tested here. See the Meyer case for an example of verifying the Jacobian+ ********************************************************************************/++#include <stdio.h>+#include <stdlib.h>+#include <math.h>+#include <float.h>++#include "lm.h"++#ifndef LM_DBL_PREC+#error Demo program assumes that levmar has been compiled with double precision, see LM_DBL_PREC!+#endif+++/* Sample functions to be minimized with LM and their Jacobians.+ * More test functions at http://www.csit.fsu.edu/~burkardt/f_src/test_nls/test_nls.html+ * Check also the CUTE problems collection at ftp://ftp.numerical.rl.ac.uk/pub/cute/;+ * CUTE is searchable through http://numawww.mathematik.tu-darmstadt.de:8081/opti/select.html+ * CUTE problems can also be solved through the AMPL web interface at http://www.ampl.com/TRYAMPL/startup.html+ *+ * Nonlinear optimization models in AMPL can be found at http://www.princeton.edu/~rvdb/ampl/nlmodels/+ */++#define ROSD 105.0++/* Rosenbrock function, global minimum at (1, 1) */+void ros(double *p, double *x, int m, int n, void *data)+{+register int i;++ for(i=0; i<n; ++i)+ x[i]=((1.0-p[0])*(1.0-p[0]) + ROSD*(p[1]-p[0]*p[0])*(p[1]-p[0]*p[0]));+}++void jacros(double *p, double *jac, int m, int n, void *data)+{+register int i, j;++ for(i=j=0; i<n; ++i){+ jac[j++]=(-2 + 2*p[0]-4*ROSD*(p[1]-p[0]*p[0])*p[0]);+ jac[j++]=(2*ROSD*(p[1]-p[0]*p[0]));+ }+}+++#define MODROSLAM 1E02+/* Modified Rosenbrock problem, global minimum at (1, 1) */+void modros(double *p, double *x, int m, int n, void *data)+{+register int i;++ for(i=0; i<n; i+=3){+ x[i]=10*(p[1]-p[0]*p[0]);+ x[i+1]=1.0-p[0];+ x[i+2]=MODROSLAM;+ }+}++void jacmodros(double *p, double *jac, int m, int n, void *data)+{+register int i, j;++ for(i=j=0; i<n; i+=3){+ jac[j++]=-20.0*p[0];+ jac[j++]=10.0;++ jac[j++]=-1.0;+ jac[j++]=0.0;++ jac[j++]=0.0;+ jac[j++]=0.0;+ }+}+++/* Powell's function, minimum at (0, 0) */+void powell(double *p, double *x, int m, int n, void *data)+{+register int i;++ for(i=0; i<n; i+=2){+ x[i]=p[0];+ x[i+1]=10.0*p[0]/(p[0]+0.1) + 2*p[1]*p[1];+ }+}++void jacpowell(double *p, double *jac, int m, int n, void *data)+{+register int i, j;++ for(i=j=0; i<n; i+=2){+ jac[j++]=1.0;+ jac[j++]=0.0;++ jac[j++]=1.0/((p[0]+0.1)*(p[0]+0.1));+ jac[j++]=4.0*p[1];+ }+}++/* Wood's function, minimum at (1, 1, 1, 1) */+void wood(double *p, double *x, int m, int n, void *data)+{+register int i;++ for(i=0; i<n; i+=6){+ x[i]=10.0*(p[1] - p[0]*p[0]);+ x[i+1]=1.0 - p[0];+ x[i+2]=sqrt(90.0)*(p[3] - p[2]*p[2]);+ x[i+3]=1.0 - p[2];+ x[i+4]=sqrt(10.0)*(p[1]+p[3] - 2.0);+ x[i+5]=(p[1] - p[3])/sqrt(10.0);+ }+}++/* Meyer's (reformulated) problem, minimum at (2.48, 6.18, 3.45) */+void meyer(double *p, double *x, int m, int n, void *data)+{+register int i;+double ui;++ for(i=0; i<n; ++i){+ ui=0.45+0.05*i;+ x[i]=p[0]*exp(10.0*p[1]/(ui+p[2]) - 13.0);+ }+}++void jacmeyer(double *p, double *jac, int m, int n, void *data)+{+register int i, j;+double ui, tmp;++ for(i=j=0; i<n; ++i){+ ui=0.45+0.05*i;+ tmp=exp(10.0*p[1]/(ui+p[2]) - 13.0);++ jac[j++]=tmp;+ jac[j++]=10.0*p[0]*tmp/(ui+p[2]);+ jac[j++]=-10.0*p[0]*p[1]*tmp/((ui+p[2])*(ui+p[2]));+ }+}++/* helical valley function, minimum at (1.0, 0.0, 0.0) */+#ifndef M_PI+#define M_PI 3.14159265358979323846 /* pi */+#endif++void helval(double *p, double *x, int m, int n, void *data)+{+double theta;++ if(p[0]<0.0)+ theta=atan(p[1]/p[0])/(2.0*M_PI) + 0.5;+ else if(0.0<p[0])+ theta=atan(p[1]/p[0])/(2.0*M_PI);+ else+ theta=(p[1]>=0)? 0.25 : -0.25;++ x[0]=10.0*(p[2] - 10.0*theta);+ x[1]=10.0*(sqrt(p[0]*p[0] + p[1]*p[1]) - 1.0);+ x[2]=p[2];+}++void jachelval(double *p, double *jac, int m, int n, void *data)+{+register int i=0;+double tmp;++ tmp=p[0]*p[0] + p[1]*p[1];++ jac[i++]=50.0*p[1]/(M_PI*tmp);+ jac[i++]=-50.0*p[0]/(M_PI*tmp);+ jac[i++]=10.0;++ jac[i++]=10.0*p[0]/sqrt(tmp);+ jac[i++]=10.0*p[1]/sqrt(tmp);+ jac[i++]=0.0;++ jac[i++]=0.0;+ jac[i++]=0.0;+ jac[i++]=1.0;+}++/* Boggs - Tolle problem 3 (linearly constrained), minimum at (-0.76744, 0.25581, 0.62791, -0.11628, 0.25581)+ * constr1: p[0] + 3*p[1] = 0;+ * constr2: p[2] + p[3] - 2*p[4] = 0;+ * constr3: p[1] - p[4] = 0;+ */+void bt3(double *p, double *x, int m, int n, void *data)+{+register int i;+double t1, t2, t3, t4;++ t1=p[0]-p[1];+ t2=p[1]+p[2]-2.0;+ t3=p[3]-1.0;+ t4=p[4]-1.0;++ for(i=0; i<n; ++i)+ x[i]=t1*t1 + t2*t2 + t3*t3 + t4*t4;+}++void jacbt3(double *p, double *jac, int m, int n, void *data)+{+register int i, j;+double t1, t2, t3, t4;++ t1=p[0]-p[1];+ t2=p[1]+p[2]-2.0;+ t3=p[3]-1.0;+ t4=p[4]-1.0;++ for(i=j=0; i<n; ++i){+ jac[j++]=2.0*t1;+ jac[j++]=2.0*(t2-t1);+ jac[j++]=2.0*t2;+ jac[j++]=2.0*t3;+ jac[j++]=2.0*t4;+ }+}++/* Hock - Schittkowski problem 28 (linearly constrained), minimum at (0.5, -0.5, 0.5)+ * constr1: p[0] + 2*p[1] + 3*p[2] = 1;+ */+void hs28(double *p, double *x, int m, int n, void *data)+{+register int i;+double t1, t2;++ t1=p[0]+p[1];+ t2=p[1]+p[2];++ for(i=0; i<n; ++i)+ x[i]=t1*t1 + t2*t2;+}++void jachs28(double *p, double *jac, int m, int n, void *data)+{+register int i, j;+double t1, t2;++ t1=p[0]+p[1];+ t2=p[1]+p[2];++ for(i=j=0; i<n; ++i){+ jac[j++]=2.0*t1;+ jac[j++]=2.0*(t1+t2);+ jac[j++]=2.0*t2;+ }+}++/* Hock - Schittkowski problem 48 (linearly constrained), minimum at (1.0, 1.0, 1.0, 1.0, 1.0)+ * constr1: sum {i in 0..4} p[i] = 5;+ * constr2: p[2] - 2*(p[3]+p[4]) = -3;+ */+void hs48(double *p, double *x, int m, int n, void *data)+{+register int i;+double t1, t2, t3;++ t1=p[0]-1.0;+ t2=p[1]-p[2];+ t3=p[3]-p[4];++ for(i=0; i<n; ++i)+ x[i]=t1*t1 + t2*t2 + t3*t3;+}++void jachs48(double *p, double *jac, int m, int n, void *data)+{+register int i, j;+double t1, t2, t3;++ t1=p[0]-1.0;+ t2=p[1]-p[2];+ t3=p[3]-p[4];++ for(i=j=0; i<n; ++i){+ jac[j++]=2.0*t1;+ jac[j++]=2.0*t2;+ jac[j++]=-2.0*t2;+ jac[j++]=2.0*t3;+ jac[j++]=-2.0*t3;+ }+}++/* Hock - Schittkowski problem 51 (linearly constrained), minimum at (1.0, 1.0, 1.0, 1.0, 1.0)+ * constr1: p[0] + 3*p[1] = 4;+ * constr2: p[2] + p[3] - 2*p[4] = 0;+ * constr3: p[1] - p[4] = 0;+ */+void hs51(double *p, double *x, int m, int n, void *data)+{+register int i;+double t1, t2, t3, t4;++ t1=p[0]-p[1];+ t2=p[1]+p[2]-2.0;+ t3=p[3]-1.0;+ t4=p[4]-1.0;++ for(i=0; i<n; ++i)+ x[i]=t1*t1 + t2*t2 + t3*t3 + t4*t4;+}++void jachs51(double *p, double *jac, int m, int n, void *data)+{+register int i, j;+double t1, t2, t3, t4;++ t1=p[0]-p[1];+ t2=p[1]+p[2]-2.0;+ t3=p[3]-1.0;+ t4=p[4]-1.0;++ for(i=j=0; i<n; ++i){+ jac[j++]=2.0*t1;+ jac[j++]=2.0*(t2-t1);+ jac[j++]=2.0*t2;+ jac[j++]=2.0*t3;+ jac[j++]=2.0*t4;+ }+}++/* Hock - Schittkowski problem 01 (box constrained), minimum at (1.0, 1.0)+ * constr1: p[1]>=-1.5;+ */+void hs01(double *p, double *x, int m, int n, void *data)+{+double t;++ t=p[0]*p[0];+ x[0]=10.0*(p[1]-t);+ x[1]=1.0-p[0];+}++void jachs01(double *p, double *jac, int m, int n, void *data)+{+register int j=0;++ jac[j++]=-20.0*p[0];+ jac[j++]=10.0;++ jac[j++]=-1.0;+ jac[j++]=0.0;+}++/* Hock - Schittkowski MODIFIED problem 21 (box constrained), minimum at (2.0, 0.0)+ * constr1: 2 <= p[0] <=50;+ * constr2: -50 <= p[1] <=50;+ *+ * Original HS21 has the additional constraint 10*p[0] - p[1] >= 10; which is inactive+ * at the solution, so it is dropped here.+ */+void hs21(double *p, double *x, int m, int n, void *data)+{+ x[0]=p[0]/10.0;+ x[1]=p[1];+}++void jachs21(double *p, double *jac, int m, int n, void *data)+{+register int j=0;++ jac[j++]=0.1;+ jac[j++]=0.0;++ jac[j++]=0.0;+ jac[j++]=1.0;+}++/* Problem hatfldb (box constrained), minimum at (0.947214, 0.8, 0.64, 0.4096)+ * constri: p[i]>=0.0; (i=1..4)+ * constr5: p[1]<=0.8;+ */+void hatfldb(double *p, double *x, int m, int n, void *data)+{+register int i;++ x[0]=p[0]-1.0;++ for(i=1; i<m; ++i)+ x[i]=p[i-1]-sqrt(p[i]);+}++void jachatfldb(double *p, double *jac, int m, int n, void *data)+{+register int j=0;++ jac[j++]=1.0;+ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=0.0;++ jac[j++]=1.0;+ jac[j++]=-0.5/sqrt(p[1]);+ jac[j++]=0.0;+ jac[j++]=0.0;++ jac[j++]=0.0;+ jac[j++]=1.0;+ jac[j++]=-0.5/sqrt(p[2]);+ jac[j++]=0.0;++ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=1.0;+ jac[j++]=-0.5/sqrt(p[3]);+}++/* Problem hatfldc (box constrained), minimum at (1.0, 1.0, 1.0, 1.0)+ * constri: p[i]>=0.0; (i=1..4)+ * constri+4: p[i]<=10.0; (i=1..4)+ */+void hatfldc(double *p, double *x, int m, int n, void *data)+{+register int i;++ x[0]=p[0]-1.0;++ for(i=1; i<m-1; ++i)+ x[i]=p[i-1]-sqrt(p[i]);++ x[m-1]=p[m-1]-1.0;+}++void jachatfldc(double *p, double *jac, int m, int n, void *data)+{+register int j=0;++ jac[j++]=1.0;+ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=0.0;++ jac[j++]=1.0;+ jac[j++]=-0.5/sqrt(p[1]);+ jac[j++]=0.0;+ jac[j++]=0.0;++ jac[j++]=0.0;+ jac[j++]=1.0;+ jac[j++]=-0.5/sqrt(p[2]);+ jac[j++]=0.0;++ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=1.0;+}++/* Hock - Schittkowski (modified) problem 52 (box/linearly constrained), minimum at (-0.09, 0.03, 0.25, -0.19, 0.03)+ * constr1: p[0] + 3*p[1] = 0;+ * constr2: p[2] + p[3] - 2*p[4] = 0;+ * constr3: p[1] - p[4] = 0;+ *+ * To the above 3 constraints, we add the following 5:+ * constr4: -0.09 <= p[0];+ * constr5: 0.0 <= p[1] <= 0.3;+ * constr6: p[2] <= 0.25;+ * constr7: -0.2 <= p[3] <= 0.3;+ * constr8: 0.0 <= p[4] <= 0.3;+ *+ */+void modhs52(double *p, double *x, int m, int n, void *data)+{+ x[0]=4.0*p[0]-p[1];+ x[1]=p[1]+p[2]-2.0;+ x[2]=p[3]-1.0;+ x[3]=p[4]-1.0;+}++void jacmodhs52(double *p, double *jac, int m, int n, void *data)+{+register int j=0;++ jac[j++]=4.0;+ jac[j++]=-1.0;+ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=0.0;++ jac[j++]=0.0;+ jac[j++]=1.0;+ jac[j++]=1.0;+ jac[j++]=0.0;+ jac[j++]=0.0;++ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=1.0;+ jac[j++]=0.0;++ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=1.0;+}++/* Schittkowski (modified) problem 235 (box/linearly constrained), minimum at (-1.725, 2.9, 0.725)+ * constr1: p[0] + p[2] = -1.0;+ *+ * To the above constraint, we add the following 2:+ * constr2: p[1] - 4*p[2] = 0;+ * constr3: 0.1 <= p[1] <= 2.9;+ * constr4: 0.7 <= p[2];+ *+ */+void mods235(double *p, double *x, int m, int n, void *data)+{+ x[0]=0.1*(p[0]-1.0);+ x[1]=p[1]-p[0]*p[0];+}++void jacmods235(double *p, double *jac, int m, int n, void *data)+{+register int j=0;++ jac[j++]=0.1;+ jac[j++]=0.0;+ jac[j++]=0.0;++ jac[j++]=-2.0*p[0];+ jac[j++]=1.0;+ jac[j++]=0.0;+}++/* Boggs and Tolle modified problem 7 (box/linearly constrained), minimum at (0.7, 0.49, 0.19, 1.19, -0.2)+ * We keep the original objective function & starting point and use the following constraints:+ *+ * subject to cons1:+ * x[1]+x[2] - x[3] = 1.0;+ * subject to cons2:+ * x[2] - x[4] + x[1] = 0.0;+ * subject to cons3:+ * x[5] + x[1] = 0.5;+ * subject to cons4:+ * x[5]>=-0.3;+ * subject to cons5:+ * x[1]<=0.7;+ *+ */+void modbt7(double *p, double *x, int m, int n, void *data)+{+register int i;++ for(i=0; i<n; ++i)+ x[i]=100.0*(p[1]-p[0]*p[0])*(p[1]-p[0]*p[0]) + (p[0]-1.0)*(p[0]-1.0);+}++void jacmodbt7(double *p, double *jac, int m, int n, void *data)+{+register int i, j;++ for(i=j=0; i<m; ++i){+ jac[j++]=-400.0*(p[1]-p[0]*p[0])*p[0] + 2.0*p[0] - 2.0;+ jac[j++]=200.0*(p[1]-p[0]*p[0]);+ jac[j++]=0.0;+ jac[j++]=0.0;+ jac[j++]=0.0;+ }+}++/* Equilibrium combustion problem, constrained nonlinear equation from the book by Floudas et al.+ * Minimum at (0.0034, 31.3265, 0.0684, 0.8595, 0.0370)+ * constri: p[i]>=0.0001; (i=1..5)+ * constri+5: p[i]<=100.0; (i=1..5)+ */+void combust(double *p, double *x, int m, int n, void *data)+{+ double R, R5, R6, R7, R8, R9, R10;++ R=10;+ R5=0.193;+ R6=4.10622*1e-4;+ R7=5.45177*1e-4;+ R8=4.4975*1e-7;+ R9=3.40735*1e-5;+ R10=9.615*1e-7;++ x[0]=p[0]*p[1]+p[0]-3*p[4];+ x[1]=2*p[0]*p[1]+p[0]+3*R10*p[1]*p[1]+p[1]*p[2]*p[2]+R7*p[1]*p[2]+R9*p[1]*p[3]+R8*p[1]-R*p[4];+ x[2]=2*p[1]*p[2]*p[2]+R7*p[1]*p[2]+2*R5*p[2]*p[2]+R6*p[2]-8*p[4];+ x[3]=R9*p[1]*p[3]+2*p[3]*p[3]-4*R*p[4];+ x[4]=p[0]*p[1]+p[0]+R10*p[1]*p[1]+p[1]*p[2]*p[2]+R7*p[1]*p[2]+R9*p[1]*p[3]+R8*p[1]+R5*p[2]*p[2]+R6*p[2]+p[3]*p[3]-1.0;+}++void jaccombust(double *p, double *jac, int m, int n, void *data)+{+register int j=0;+ double R, R5, R6, R7, R8, R9, R10;++ R=10;+ R5=0.193;+ R6=4.10622*1e-4;+ R7=5.45177*1e-4;+ R8=4.4975*1e-7;+ R9=3.40735*1e-5;+ R10=9.615*1e-7;++ for(j=0; j<m*n; ++j) jac[j]=0.0;++ j=0;+ jac[j]=p[1]+1;+ jac[j+1]=p[0];+ jac[j+4]=-3;++ j+=m;+ jac[j]=2*p[1]+1;+ jac[j+1]=2*p[0]+6*R10*p[1]+p[2]*p[2]+R7*p[2]+R9*p[3]+R8;+ jac[j+2]=2*p[1]*p[2]+R7*p[1];+ jac[j+3]=R9*p[1];+ jac[j+4]=-R;++ j+=m;+ jac[j+1]=2*p[2]*p[2]+R7*p[2];+ jac[j+2]=4*p[1]*p[2]+R7*p[1]+4*R5*p[2]+R6;+ jac[j+4]=-8;++ j+=m;+ jac[j+1]=R9*p[3];+ jac[j+3]=R9*p[1]+4*p[3];+ jac[j+4]=-4*R;++ j+=m;+ jac[j]=p[1]+1;+ jac[j+1]=p[0]+2*R10*p[1]+p[2]*p[2]+R7*p[2]+R9*p[3]+R8;+ jac[j+2]=2*p[1]*p[2]+R7*p[1]+2*R5*p[2]+R6;+ jac[j+3]=R9*p[1]+2*p[3];+}++++int main()+{+register int i, j;+int problem, ret;+double p[5], // 6 is max(2, 3, 5)+ x[16]; // 16 is max(2, 3, 5, 6, 16)+int m, n;+double opts[LM_OPTS_SZ], info[LM_INFO_SZ];+char *probname[]={+ "Rosenbrock function",+ "modified Rosenbrock problem",+ "Powell's function",+ "Wood's function",+ "Meyer's (reformulated) problem",+ "helical valley function",+ "Boggs & Tolle's problem #3",+ "Hock - Schittkowski problem #28",+ "Hock - Schittkowski problem #48",+ "Hock - Schittkowski problem #51",+ "Hock - Schittkowski problem #01",+ "Hock - Schittkowski modified problem #21",+ "hatfldb problem",+ "hatfldc problem",+ "equilibrium combustion problem",+ "Hock - Schittkowski modified problem #52",+ "Schittkowski modified problem #235",+ "Boggs & Tolle modified problem #7",+};++ opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20;+ opts[4]=LM_DIFF_DELTA; // relevant only if the Jacobian is approximated using finite differences; specifies forward differencing+ //opts[4]=-LM_DIFF_DELTA; // specifies central differencing to approximate Jacobian; more accurate but more expensive to compute!++ /* uncomment the appropriate line below to select a minimization problem */+ problem=+ //0; // Rosenbrock function+ //1; // modified Rosenbrock problem+ //2; // Powell's function+ //3; // Wood's function+ 4; // Meyer's (reformulated) problem+ //5; // helical valley function+#ifdef HAVE_LAPACK+ //6; // Boggs & Tolle's problem 3+ //7; // Hock - Schittkowski problem 28+ //8; // Hock - Schittkowski problem 48+ //9; // Hock - Schittkowski problem 51+#else // no LAPACK+#ifdef _MSC_VER+#pragma message("LAPACK not available, some test problems cannot be used")+#else+#warning LAPACK not available, some test problems cannot be used+#endif // _MSC_VER++#endif /* HAVE_LAPACK */+ //10; // Hock - Schittkowski problem 01+ //11; // Hock - Schittkowski modified problem 21+ //12; // hatfldb problem+ //13; // hatfldc problem+ //14; // equilibrium combustion problem+#ifdef HAVE_LAPACK+ //15; // Hock - Schittkowski modified problem 52+ //16; // Schittkowski modified problem 235+ //17; // Boggs & Tolle modified problem #7+#endif /* HAVE_LAPACK */++ switch(problem){+ default: PRINT_ERROR("unknown problem specified (#%d)! Note that some minimization problems require LAPACK.\n", problem);+ exit(1);+ break;+ case 0:+ /* Rosenbrock function */+ m=2; n=2;+ p[0]=-1.2; p[1]=1.0;+ for(i=0; i<n; i++) x[i]=0.0;+ ret=dlevmar_der(ros, jacros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ //ret=dlevmar_dif(ros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // no Jacobian+ break;++ case 1:+ /* modified Rosenbrock problem */+ m=2; n=3;+ p[0]=-1.2; p[1]=1.0;+ for(i=0; i<n; i++) x[i]=0.0;+ ret=dlevmar_der(modros, jacmodros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ //ret=dlevmar_dif(modros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // no Jacobian+ break;++ case 2:+ /* Powell's function */+ m=2; n=2;+ p[0]=3.0; p[1]=1.0;+ for(i=0; i<n; i++) x[i]=0.0;+ ret=dlevmar_der(powell, jacpowell, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ //ret=dlevmar_dif(powell, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // no Jacobian+ break;++ case 3:+ /* Wood's function */+ m=4; n=6;+ p[0]=-3.0; p[1]=-1.0; p[2]=-3.0; p[3]=-1.0;+ for(i=0; i<n; i++) x[i]=0.0;+ ret=dlevmar_dif(wood, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // no Jacobian+ break;++ case 4:+ /* Meyer's data fitting problem */+ m=3; n=16;+ p[0]=8.85; p[1]=4.0; p[2]=2.5;+ x[0]=34.780; x[1]=28.610; x[2]=23.650; x[3]=19.630;+ x[4]=16.370; x[5]=13.720; x[6]=11.540; x[7]=9.744;+ x[8]=8.261; x[9]=7.030; x[10]=6.005; x[11]=5.147;+ x[12]=4.427; x[13]=3.820; x[14]=3.307; x[15]=2.872;+ //ret=dlevmar_der(meyer, jacmeyer, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian++ { double *work, *covar;+ work=malloc((LM_DIF_WORKSZ(m, n)+m*m)*sizeof(double));+ if(!work){+ PRINT_ERROR("memory allocation request failed in main()\n");+ exit(1);+ }+ covar=work+LM_DIF_WORKSZ(m, n);++ ret=dlevmar_dif(meyer, p, x, m, n, 1000, opts, info, work, covar, NULL); // no Jacobian, caller allocates work memory, covariance estimated++ printf("Covariance of the fit:\n");+ for(i=0; i<m; ++i){+ for(j=0; j<m; ++j)+ printf("%g ", covar[i*m+j]);+ printf("\n");+ }+ printf("\n");++ free(work);+ }++/* uncomment the following block to verify Jacobian */+/*+ {+ double err[16];+ dlevmar_chkjac(meyer, jacmeyer, p, m, n, NULL, err);+ for(i=0; i<n; ++i) printf("gradient %d, err %g\n", i, err[i]);+ }+*/++ break;++ case 5:+ /* helical valley function */+ m=3; n=3;+ p[0]=-1.0; p[1]=0.0; p[2]=0.0;+ for(i=0; i<n; i++) x[i]=0.0;+ ret=dlevmar_der(helval, jachelval, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ //ret=dlevmar_dif(helval, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // no Jacobian+ break;++#ifdef HAVE_LAPACK+ case 6:+ /* Boggs-Tolle problem 3 */+ m=5; n=5;+ p[0]=2.0; p[1]=2.0; p[2]=2.0;+ p[3]=2.0; p[4]=2.0;+ for(i=0; i<n; i++) x[i]=0.0;++ {+ double A[3*5]={1.0, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, -2.0, 0.0, 1.0, 0.0, 0.0, -1.0},+ b[3]={0.0, 0.0, 0.0};++ ret=dlevmar_lec_der(bt3, jacbt3, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian+ //ret=dlevmar_lec_dif(bt3, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian+ }+ break;+ case 7:+ /* Hock - Schittkowski problem 28 */+ m=3; n=3;+ p[0]=-4.0; p[1]=1.0; p[2]=1.0;+ for(i=0; i<n; i++) x[i]=0.0;++ {+ double A[1*3]={1.0, 2.0, 3.0},+ b[1]={1.0};++ ret=dlevmar_lec_der(hs28, jachs28, p, x, m, n, A, b, 1, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian+ //ret=dlevmar_lec_dif(hs28, p, x, m, n, A, b, 1, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian+ }+ break;+ case 8:+ /* Hock - Schittkowski problem 48 */+ m=5; n=5;+ p[0]=3.0; p[1]=5.0; p[2]=-3.0;+ p[3]=2.0; p[4]=-2.0;+ for(i=0; i<n; i++) x[i]=0.0;++ {+ double A[2*5]={1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 1.0, -2.0, -2.0},+ b[2]={5.0, -3.0};++ ret=dlevmar_lec_der(hs48, jachs48, p, x, m, n, A, b, 2, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian+ //ret=dlevmar_lec_dif(hs48, p, x, m, n, A, b, 2, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian+ }+ break;+ case 9:+ /* Hock - Schittkowski problem 51 */+ m=5; n=5;+ p[0]=2.5; p[1]=0.5; p[2]=2.0;+ p[3]=-1.0; p[4]=0.5;+ for(i=0; i<n; i++) x[i]=0.0;++ {+ double A[3*5]={1.0, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, -2.0, 0.0, 1.0, 0.0, 0.0, -1.0},+ b[3]={4.0, 0.0, 0.0};++ ret=dlevmar_lec_der(hs51, jachs51, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian+ //ret=dlevmar_lec_dif(hs51, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian+ }+ break;+#endif /* HAVE_LAPACK */++ case 10:+ /* Hock - Schittkowski problem 01 */+ m=2; n=2;+ p[0]=-2.0; p[1]=1.0;+ for(i=0; i<n; i++) x[i]=0.0;+ //ret=dlevmar_der(hs01, jachs01, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ {+ double lb[2], ub[2];++ lb[0]=-DBL_MAX; lb[1]=-1.5;+ ub[0]=ub[1]=DBL_MAX;++ ret=dlevmar_bc_der(hs01, jachs01, p, x, m, n, lb, ub, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ }+ break;+ case 11:+ /* Hock - Schittkowski (modified) problem 21 */+ m=2; n=2;+ p[0]=-1.0; p[1]=-1.0;+ for(i=0; i<n; i++) x[i]=0.0;+ //ret=dlevmar_der(hs21, jachs21, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ {+ double lb[2], ub[2];++ lb[0]=2.0; lb[1]=-50.0;+ ub[0]=50.0; ub[1]=50.0;++ ret=dlevmar_bc_der(hs21, jachs21, p, x, m, n, lb, ub, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ }+ break;+ case 12:+ /* hatfldb problem */+ m=4; n=4;+ p[0]=p[1]=p[2]=p[3]=0.1;+ for(i=0; i<n; i++) x[i]=0.0;+ //ret=dlevmar_der(hatfldb, jachatfldb, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ {+ double lb[4], ub[4];++ lb[0]=lb[1]=lb[2]=lb[3]=0.0;++ ub[0]=ub[2]=ub[3]=DBL_MAX;+ ub[1]=0.8;++ ret=dlevmar_bc_der(hatfldb, jachatfldb, p, x, m, n, lb, ub, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ }+ break;+ case 13:+ /* hatfldc problem */+ m=4; n=4;+ p[0]=p[1]=p[2]=p[3]=0.9;+ for(i=0; i<n; i++) x[i]=0.0;+ //ret=dlevmar_der(hatfldc, jachatfldc, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ {+ double lb[4], ub[4];++ lb[0]=lb[1]=lb[2]=lb[3]=0.0;++ ub[0]=ub[1]=ub[2]=ub[3]=10.0;++ ret=dlevmar_bc_der(hatfldc, jachatfldc, p, x, m, n, lb, ub, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ }+ break;+ case 14:+ /* equilibrium combustion problem */+ m=5; n=5;+ p[0]=p[1]=p[2]=p[3]=p[4]=0.0001;+ for(i=0; i<n; i++) x[i]=0.0;+ //ret=dlevmar_der(combust, jaccombust, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ {+ double lb[5], ub[5];++ lb[0]=lb[1]=lb[2]=lb[3]=lb[4]=0.0001;++ ub[0]=ub[1]=ub[2]=ub[3]=ub[4]=100.0;++ ret=dlevmar_bc_der(combust, jaccombust, p, x, m, n, lb, ub, 5000, opts, info, NULL, NULL, NULL); // with analytic Jacobian+ }+ break;+#ifdef HAVE_LAPACK+ case 15:+ /* Hock - Schittkowski modified problem 52 */+ m=5; n=4;+ p[0]=2.0; p[1]=2.0; p[2]=2.0;+ p[3]=2.0; p[4]=2.0;+ for(i=0; i<n; i++) x[i]=0.0;++ {+ double A[3*5]={1.0, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, -2.0, 0.0, 1.0, 0.0, 0.0, -1.0},+ b[3]={0.0, 0.0, 0.0};++ double lb[5], ub[5];++ double weights[5]={2000.0, 2000.0, 2000.0, 2000.0, 2000.0}; // penalty terms weights++ lb[0]=-0.09; lb[1]=0.0; lb[2]=-DBL_MAX; lb[3]=-0.2; lb[4]=0.0;+ ub[0]=DBL_MAX; ub[1]=0.3; ub[2]=0.25; ub[3]=0.3; ub[4]=0.3;++ ret=dlevmar_blec_der(modhs52, jacmodhs52, p, x, m, n, lb, ub, A, b, 3, weights, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, analytic Jacobian+ //ret=dlevmar_blec_dif(modhs52, p, x, m, n, lb, ub, A, b, 3, weights, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, no Jacobian+ }+ break;+ case 16:+ /* Schittkowski modified problem 235 */+ m=3; n=2;+ p[0]=-2.0; p[1]=3.0; p[2]=1.0;+ for(i=0; i<n; i++) x[i]=0.0;++ {+ double A[2*3]={1.0, 0.0, 1.0, 0.0, 1.0, -4.0},+ b[2]={-1.0, 0.0};++ double lb[3], ub[3];++ lb[0]=-DBL_MAX; lb[1]=0.1; lb[2]=0.7;+ ub[0]=DBL_MAX; ub[1]=2.9; ub[2]=DBL_MAX;++ ret=dlevmar_blec_der(mods235, jacmods235, p, x, m, n, lb, ub, A, b, 2, NULL, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, analytic Jacobian+ //ret=dlevmar_blec_dif(mods235, p, x, m, n, lb, ub, A, b, 2, NULL, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, no Jacobian+ }+ break;+ case 17:+ /* Boggs & Tolle modified problem 7 */+ m=5; n=5;+ p[0]=-2.0; p[1]=1.0; p[2]=1.0; p[3]=1.0; p[4]=1.0;+ for(i=0; i<n; i++) x[i]=0.0;++ {+ double A[3*5]={1.0, 1.0, -1.0, 0.0, 0.0, 1.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0},+ b[3]={1.0, 0.0, 0.5};++ double lb[5], ub[5];++ lb[0]=-DBL_MAX; lb[1]=-DBL_MAX; lb[2]=-DBL_MAX; lb[3]=-DBL_MAX; lb[4]=-0.3;+ ub[0]=0.7; ub[1]= DBL_MAX; ub[2]= DBL_MAX; ub[3]= DBL_MAX; ub[4]=DBL_MAX;++ ret=dlevmar_blec_der(modbt7, jacmodbt7, p, x, m, n, lb, ub, A, b, 3, NULL, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, analytic Jacobian+ //ret=dlevmar_blec_dif(modbt7, p, x, m, n, lb, ub, A, b, 3, NULL, 10000, opts, info, NULL, NULL, NULL); // box & lin. constraints, no Jacobian+ }+ break;+#endif /* HAVE_LAPACK */+ } /* switch */++ printf("Results for %s:\n", probname[problem]);+ printf("Levenberg-Marquardt returned %d in %g iter, reason %g\nSolution: ", ret, info[5], info[6]);+ for(i=0; i<m; ++i)+ printf("%.7g ", p[i]);+ printf("\n\nMinimization info:\n");+ for(i=0; i<LM_INFO_SZ; ++i)+ printf("%g ", info[i]);+ printf("\n");++ return 0;+}
+ levmar-2.4/lmlec.c view
@@ -0,0 +1,80 @@+/////////////////////////////////////////////////////////////////////////////////+// +// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++/*******************************************************************************+ * Wrappers for linearly constrained Levenberg-Marquardt minimization. The same+ * core code is used with appropriate #defines to derive single and double+ * precision versions, see also lmlec_core.c+ *******************************************************************************/++#include <stdio.h>+#include <stdlib.h>+#include <math.h>++#include "lm.h"+#include "misc.h"+++#ifndef HAVE_LAPACK++#ifdef _MSC_VER+#pragma message("Linearly constrained optimization requires LAPACK and was not compiled!")+#else+#warning Linearly constrained optimization requires LAPACK and was not compiled!+#endif // _MSC_VER++#else // LAPACK present++#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!+#endif+++#ifdef LM_SNGL_PREC+/* single precision (float) definitions */+#define LM_REAL float+#define LM_PREFIX s++#define __SUBCNST(x) x##F+#define LM_CNST(x) __SUBCNST(x) // force substitution++#include "lmlec_core.c" // read in core code++#undef LM_REAL+#undef LM_PREFIX+#undef __SUBCNST+#undef LM_CNST+#endif /* LM_SNGL_PREC */++#ifdef LM_DBL_PREC+/* double precision definitions */+#define LM_REAL double+#define LM_PREFIX d++#define LM_CNST(x) (x)++#include "lmlec_core.c" // read in core code++#undef LM_REAL+#undef LM_PREFIX+#undef LM_CNST+#endif /* LM_DBL_PREC */++#endif /* HAVE_LAPACK */+
+ levmar-2.4/lmlec_core.c view
@@ -0,0 +1,657 @@+/////////////////////////////////////////////////////////////////////////////////+//+// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++#ifndef LM_REAL // not included by lmlec.c+#error This file should not be compiled directly!+#endif+++/* precision-specific definitions */+#define LMLEC_DATA LM_ADD_PREFIX(lmlec_data)+#define LMLEC_ELIM LM_ADD_PREFIX(lmlec_elim)+#define LMLEC_FUNC LM_ADD_PREFIX(lmlec_func)+#define LMLEC_JACF LM_ADD_PREFIX(lmlec_jacf)+#define LEVMAR_LEC_DER LM_ADD_PREFIX(levmar_lec_der)+#define LEVMAR_LEC_DIF LM_ADD_PREFIX(levmar_lec_dif)+#define LEVMAR_DER LM_ADD_PREFIX(levmar_der)+#define LEVMAR_DIF LM_ADD_PREFIX(levmar_dif)+#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)+#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)+#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)++#define GEQP3 LM_MK_LAPACK_NAME(geqp3)+#define ORGQR LM_MK_LAPACK_NAME(orgqr)+#define TRTRI LM_MK_LAPACK_NAME(trtri)++struct LMLEC_DATA{+ LM_REAL *c, *Z, *p, *jac;+ int ncnstr;+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);+ void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);+ void *adata;+};++/* prototypes for LAPACK routines */+extern int GEQP3(int *m, int *n, LM_REAL *a, int *lda, int *jpvt,+ LM_REAL *tau, LM_REAL *work, int *lwork, int *info);++extern int ORGQR(int *m, int *n, int *k, LM_REAL *a, int *lda, LM_REAL *tau,+ LM_REAL *work, int *lwork, int *info);++extern int TRTRI(char *uplo, char *diag, int *n, LM_REAL *a, int *lda, int *info);++/*+ * This function implements an elimination strategy for linearly constrained+ * optimization problems. The strategy relies on QR decomposition to transform+ * an optimization problem constrained by Ax=b to an equivalent, unconstrained+ * one. Also referred to as "null space" or "reduced Hessian" method.+ * See pp. 430-433 (chap. 15) of "Numerical Optimization" by Nocedal-Wright+ * for details.+ *+ * A is mxn with m<=n and rank(A)=m+ * Two matrices Y and Z of dimensions nxm and nx(n-m) are computed from A^T so that+ * their columns are orthonormal and every x can be written as x=Y*b + Z*x_z=+ * c + Z*x_z, where c=Y*b is a fixed vector of dimension n and x_z is an+ * arbitrary vector of dimension n-m. Then, the problem of minimizing f(x)+ * subject to Ax=b is equivalent to minimizing f(c + Z*x_z) with no constraints.+ * The computed Y and Z are such that any solution of Ax=b can be written as+ * x=Y*x_y + Z*x_z for some x_y, x_z. Furthermore, A*Y is nonsingular, A*Z=0+ * and Z spans the null space of A.+ *+ * The function accepts A, b and computes c, Y, Z. If b or c is NULL, c is not+ * computed. Also, Y can be NULL in which case it is not referenced.+ * The function returns an error code (<0) in case of error or A's computed rank if successful.+ *+ */+static int LMLEC_ELIM(LM_REAL *A, LM_REAL *b, LM_REAL *c, LM_REAL *Y, LM_REAL *Z, int m, int n)+{+static LM_REAL eps=LM_CNST(-1.0);++LM_REAL *buf=NULL;+LM_REAL *a, *tau, *work, *r, aux;+register LM_REAL tmp;+int a_sz, jpvt_sz, tau_sz, r_sz, Y_sz, worksz;+int info, rank, *jpvt, tot_sz, mintmn, tm, tn;+register int i, j, k;++ if(m>n){+ PRINT_ERROR(RCAT("matrix of constraints cannot have more rows than columns in", LMLEC_ELIM) "()!\n");+ return LM_ERROR_CONSTRAINT_MATRIX_ROWS_GT_COLS;+ }++ tm=n; tn=m; // transpose dimensions+ mintmn=m;++ /* calculate required memory size */+ worksz=-1; // workspace query. Optimal work size is returned in aux+ //ORGQR((int *)&tm, (int *)&tm, (int *)&mintmn, NULL, (int *)&tm, NULL, (LM_REAL *)&aux, &worksz, &info);+ GEQP3((int *)&tm, (int *)&tn, NULL, (int *)&tm, NULL, NULL, (LM_REAL *)&aux, (int *)&worksz, &info);+ worksz=(int)aux;+ a_sz=tm*tm; // tm*tn is enough for xgeqp3()+ jpvt_sz=tn;+ tau_sz=mintmn;+ r_sz=mintmn*mintmn; // actually smaller if a is not of full row rank+ Y_sz=(Y)? 0 : tm*tn;++ tot_sz=(a_sz + tau_sz + r_sz + worksz + Y_sz)*sizeof(LM_REAL) + jpvt_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */+ buf=(LM_REAL *)malloc(tot_sz); /* allocate a "big" memory chunk at once */+ if(!buf){+ PRINT_ERROR(RCAT("Memory allocation request failed in ", LMLEC_ELIM) "()\n");+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }++ a=buf;+ tau=a+a_sz;+ r=tau+tau_sz;+ work=r+r_sz;+ if(!Y){+ Y=work+worksz;+ jpvt=(int *)(Y+Y_sz);+ }+ else+ jpvt=(int *)(work+worksz);++ /* copy input array so that LAPACK won't destroy it. Note that copying is+ * done in row-major order, which equals A^T in column-major+ */+ for(i=0; i<tm*tn; ++i)+ a[i]=A[i];++ /* clear jpvt */+ for(i=0; i<jpvt_sz; ++i) jpvt[i]=0;++ /* rank revealing QR decomposition of A^T*/+ GEQP3((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, (int *)&worksz, &info);+ //dgeqpf_((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, &info);+ /* error checking */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQP3) " in ", LMLEC_ELIM) "()\n", -info);+ }+ else if(info>0){+ PRINT_ERROR(RCAT(RCAT("unknown LAPACK error (%d) for ", GEQP3) " in ", LMLEC_ELIM) "()\n", info);+ }+ free(buf);+ return LM_ERROR_LAPACK_ERROR;+ }+ /* the upper triangular part of a now contains the upper triangle of the unpermuted R */++ if(eps<0.0){+ LM_REAL aux;++ /* compute machine epsilon. DBL_EPSILON should do also */+ for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))+ ;+ eps*=LM_CNST(2.0);+ }++ tmp=tm*LM_CNST(10.0)*eps*FABS(a[0]); // threshold. tm is max(tm, tn)+ tmp=(tmp>LM_CNST(1E-12))? tmp : LM_CNST(1E-12); // ensure that threshold is not too small+ /* compute A^T's numerical rank by counting the non-zeros in R's diagonal */+ for(i=rank=0; i<mintmn; ++i)+ if(a[i*(tm+1)]>tmp || a[i*(tm+1)]<-tmp) ++rank; /* loop across R's diagonal elements */+ else break; /* diagonal is arranged in absolute decreasing order */++ if(rank<tn){+ PRINT_ERROR(RCAT("\nConstraints matrix in ", LMLEC_ELIM) "() is not of full row rank (i.e. %d < %d)!\n"+ "Make sure that you do not specify redundant or inconsistent constraints.\n\n", rank, tn);+ free(buf);+ return LM_ERROR_CONSTRAINT_MATRIX_NOT_FULL_ROW_RANK;+ }++ /* compute the permuted inverse transpose of R */+ /* first, copy R from the upper triangular part of a to r. R is rank x rank */+ for(j=0; j<rank; ++j){+ for(i=0; i<=j; ++i)+ r[i+j*rank]=a[i+j*tm];+ for(i=j+1; i<rank; ++i)+ r[i+j*rank]=0.0; // lower part is zero+ }++ /* compute the inverse */+ TRTRI("U", "N", (int *)&rank, r, (int *)&rank, &info);+ /* error checking */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRI) " in ", LMLEC_ELIM) "()\n", -info);+ }+ else if(info>0){+ PRINT_ERROR(RCAT(RCAT("A(%d, %d) is exactly zero for ", TRTRI) " (singular matrix) in ", LMLEC_ELIM) "()\n", info, info);+ }+ free(buf);+ return LM_ERROR_LAPACK_ERROR;+ }+ /* then, transpose r in place */+ for(i=0; i<rank; ++i)+ for(j=i+1; j<rank; ++j){+ tmp=r[i+j*rank];+ k=j+i*rank;+ r[i+j*rank]=r[k];+ r[k]=tmp;+ }++ /* finally, permute R^-T using Y as intermediate storage */+ for(j=0; j<rank; ++j)+ for(i=0, k=jpvt[j]-1; i<rank; ++i)+ Y[i+k*rank]=r[i+j*rank];++ for(i=0; i<rank*rank; ++i) // copy back to r+ r[i]=Y[i];++ /* resize a to be tm x tm, filling with zeroes */+ for(i=tm*tn; i<tm*tm; ++i)+ a[i]=0.0;++ /* compute Q in a as the product of elementary reflectors. Q is tm x tm */+ ORGQR((int *)&tm, (int *)&tm, (int *)&mintmn, a, (int *)&tm, tau, work, &worksz, &info);+ /* error checking */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", ORGQR) " in ", LMLEC_ELIM) "()\n", -info);+ }+ else if(info>0){+ PRINT_ERROR(RCAT(RCAT("unknown LAPACK error (%d) for ", ORGQR) " in ", LMLEC_ELIM) "()\n", info);+ }+ free(buf);+ return LM_ERROR_LAPACK_ERROR;+ }++ /* compute Y=Q_1*R^-T*P^T. Y is tm x rank */+ for(i=0; i<tm; ++i)+ for(j=0; j<rank; ++j){+ for(k=0, tmp=0.0; k<rank; ++k)+ tmp+=a[i+k*tm]*r[k+j*rank];+ Y[i*rank+j]=tmp;+ }++ if(b && c){+ /* compute c=Y*b */+ for(i=0; i<tm; ++i){+ for(j=0, tmp=0.0; j<rank; ++j)+ tmp+=Y[i*rank+j]*b[j];++ c[i]=tmp;+ }+ }++ /* copy Q_2 into Z. Z is tm x (tm-rank) */+ for(j=0; j<tm-rank; ++j)+ for(i=0, k=j+rank; i<tm; ++i)+ Z[i*(tm-rank)+j]=a[i+k*tm];++ free(buf);++ return rank;+}++/* constrained measurements: given pp, compute the measurements at c + Z*pp */+static void LMLEC_FUNC(LM_REAL *pp, LM_REAL *hx, int mm, int n, void *adata)+{+struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;+int m;+register int i, j;+register LM_REAL sum;+LM_REAL *c, *Z, *p, *Zimm;++ m=mm+data->ncnstr;+ c=data->c;+ Z=data->Z;+ p=data->p;+ /* p=c + Z*pp */+ for(i=0; i<m; ++i){+ Zimm=Z+i*mm;+ for(j=0, sum=c[i]; j<mm; ++j)+ sum+=Zimm[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];+ p[i]=sum;+ }++ (*(data->func))(p, hx, m, n, data->adata);+}++/* constrained Jacobian: given pp, compute the Jacobian at c + Z*pp+ * Using the chain rule, the Jacobian with respect to pp equals the+ * product of the Jacobian with respect to p (at c + Z*pp) times Z+ */+static void LMLEC_JACF(LM_REAL *pp, LM_REAL *jacjac, int mm, int n, void *adata)+{+struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;+int m;+register int i, j, l;+register LM_REAL sum, *aux1, *aux2;+LM_REAL *c, *Z, *p, *jac;++ m=mm+data->ncnstr;+ c=data->c;+ Z=data->Z;+ p=data->p;+ jac=data->jac;+ /* p=c + Z*pp */+ for(i=0; i<m; ++i){+ aux1=Z+i*mm;+ for(j=0, sum=c[i]; j<mm; ++j)+ sum+=aux1[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];+ p[i]=sum;+ }++ (*(data->jacf))(p, jac, m, n, data->adata);++ /* compute jac*Z in jacjac */+ if(n*m<=__BLOCKSZ__SQ){ // this is a small problem+ /* This is the straightforward way to compute jac*Z. However, due to+ * its noncontinuous memory access pattern, it incures many cache misses when+ * applied to large minimization problems (i.e. problems involving a large+ * number of free variables and measurements), in which jac is too large to+ * fit in the L1 cache. For such problems, a cache-efficient blocking scheme+ * is preferable. On the other hand, the straightforward algorithm is faster+ * on small problems since in this case it avoids the overheads of blocking.+ */++ for(i=0; i<n; ++i){+ aux1=jac+i*m;+ aux2=jacjac+i*mm;+ for(j=0; j<mm; ++j){+ for(l=0, sum=0.0; l<m; ++l)+ sum+=aux1[l]*Z[l*mm+j]; // sum+=jac[i*m+l]*Z[l*mm+j];++ aux2[j]=sum; // jacjac[i*mm+j]=sum;+ }+ }+ }+ else{ // this is a large problem+ /* Cache efficient computation of jac*Z based on blocking+ */+#define __MIN__(x, y) (((x)<=(y))? (x) : (y))+ register int jj, ll;++ for(jj=0; jj<mm; jj+=__BLOCKSZ__){+ for(i=0; i<n; ++i){+ aux1=jacjac+i*mm;+ for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j)+ aux1[j]=0.0; //jacjac[i*mm+j]=0.0;+ }++ for(ll=0; ll<m; ll+=__BLOCKSZ__){+ for(i=0; i<n; ++i){+ aux1=jacjac+i*mm; aux2=jac+i*m;+ for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j){+ sum=0.0;+ for(l=ll; l<__MIN__(ll+__BLOCKSZ__, m); ++l)+ sum+=aux2[l]*Z[l*mm+j]; //jac[i*m+l]*Z[l*mm+j];+ aux1[j]+=sum; //jacjac[i*mm+j]+=sum;+ }+ }+ }+ }+ }+}+#undef __MIN__+++/*+ * This function is similar to LEVMAR_DER except that the minimization+ * is performed subject to the linear constraints A p=b, A is kxm, b kx1+ *+ * This function requires an analytic Jacobian. In case the latter is unavailable,+ * use LEVMAR_LEC_DIF() bellow+ *+ */+int LEVMAR_LEC_DER(+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */+ void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), /* function to evaluate the Jacobian \part x / \part p */+ LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */+ LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */+ int m, /* I: parameter vector dimension (i.e. #unknowns) */+ int n, /* I: measurement vector dimension */+ LM_REAL *A, /* I: constraints matrix, kxm */+ LM_REAL *b, /* I: right hand constraints vector, kx1 */+ int k, /* I: number of constraints (i.e. A's #rows) */+ int itmax, /* I: maximum number of iterations */+ LM_REAL opts[4], /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,+ * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used+ */+ LM_REAL info[LM_INFO_SZ],+ /* O: information regarding the minimization. Set to NULL if don't care+ * info[0]= ||e||_2 at initial p.+ * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.+ * info[5]= # iterations,+ * info[6]=reason for terminating: 1 - stopped by small gradient J^T e+ * 2 - stopped by small Dp+ * 3 - stopped by itmax+ * 4 - singular matrix. Restart from current p with increased mu+ * 5 - no further error reduction is possible. Restart with increased mu+ * 6 - stopped by small ||e||_2+ * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error+ * info[7]= # function evaluations+ * info[8]= # Jacobian evaluations+ * info[9]= # linear systems solved, i.e. # attempts for reducing error+ */+ LM_REAL *work, /* working memory at least LM_LEC_DER_WORKSZ() reals large, allocated if NULL */+ LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */+ void *adata) /* pointer to possibly additional data, passed uninterpreted to func & jacf.+ * Set to NULL if not needed+ */+{+ struct LMLEC_DATA data;+ LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */+ int mm, ret;+ register int i, j;+ register LM_REAL tmp;+ LM_REAL locinfo[LM_INFO_SZ];++ if(!jacf){+ PRINT_ERROR(RCAT("No function specified for computing the Jacobian in ", LEVMAR_LEC_DER)+ RCAT("().\nIf no such function is available, use ", LEVMAR_LEC_DIF) RCAT("() rather than ", LEVMAR_LEC_DER) "()\n");+ return LM_ERROR_NO_JACOBIAN;+ }++ mm=m-k;++ if(n<mm){+ PRINT_ERROR(LCAT(LEVMAR_LEC_DER, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k, m);+ return LM_ERROR_TOO_FEW_MEASUREMENTS;+ }++ ptr=(LM_REAL *)malloc((2*m + m*mm + n*m + mm)*sizeof(LM_REAL));+ if(!ptr){+ PRINT_ERROR(LCAT(LEVMAR_LEC_DER, "(): memory allocation request failed\n"));+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }+ data.p=p;+ p0=ptr;+ data.c=p0+m;+ data.Z=Z=data.c+m;+ data.jac=data.Z+m*mm;+ pp=data.jac+n*m;+ data.ncnstr=k;+ data.func=func;+ data.jacf=jacf;+ data.adata=adata;++ ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z+ if(ret<0){+ free(ptr);+ return ret;+ }++ /* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)+ * Due to orthogonality, Z^T Z = I and the last equation+ * becomes pp=Z^T*(p-c). Also, save the starting p in p0+ */+ for(i=0; i<m; ++i){+ p0[i]=p[i];+ p[i]-=data.c[i];+ }++ /* Z^T*(p-c) */+ for(i=0; i<mm; ++i){+ for(j=0, tmp=0.0; j<m; ++j)+ tmp+=Z[j*mm+i]*p[j];+ pp[i]=tmp;+ }++ /* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */+ for(i=0; i<m; ++i){+ Zimm=Z+i*mm;+ for(j=0, tmp=data.c[i]; j<mm; ++j)+ tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];+ if(FABS(tmp-p0[i])>LM_CNST(1E-03))+ PRINT_ERROR(RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DER) "()! [%.10g reset to %.10g]\n",+ i, p0[i], tmp);+ }++ if(!info) info=locinfo; /* make sure that LEVMAR_DER() is called with non-null info */+ /* note that covariance computation is not requested from LEVMAR_DER() */+ ret=LEVMAR_DER(LMLEC_FUNC, LMLEC_JACF, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);++ /* p=c + Z*pp */+ for(i=0; i<m; ++i){+ Zimm=Z+i*mm;+ for(j=0, tmp=data.c[i]; j<mm; ++j)+ tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];+ p[i]=tmp;+ }++ /* compute the covariance from the Jacobian in data.jac */+ if(covar){+ LEVMAR_TRANS_MAT_MAT_MULT(data.jac, covar, n, m); /* covar = J^T J */+ LEVMAR_COVAR(covar, covar, info[1], m, n);+ }++ free(ptr);++ return ret;+}++/* Similar to the LEVMAR_LEC_DER() function above, except that the Jacobian is approximated+ * with the aid of finite differences (forward or central, see the comment for the opts argument)+ */+int LEVMAR_LEC_DIF(+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */+ LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */+ LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */+ int m, /* I: parameter vector dimension (i.e. #unknowns) */+ int n, /* I: measurement vector dimension */+ LM_REAL *A, /* I: constraints matrix, kxm */+ LM_REAL *b, /* I: right hand constraints vector, kx1 */+ int k, /* I: number of constraints (i.e. A's #rows) */+ int itmax, /* I: maximum number of iterations */+ LM_REAL opts[5], /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the+ * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and+ * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.+ * If \delta<0, the Jacobian is approximated with central differences which are more accurate+ * (but slower!) compared to the forward differences employed by default.+ */+ LM_REAL info[LM_INFO_SZ],+ /* O: information regarding the minimization. Set to NULL if don't care+ * info[0]= ||e||_2 at initial p.+ * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.+ * info[5]= # iterations,+ * info[6]=reason for terminating: 1 - stopped by small gradient J^T e+ * 2 - stopped by small Dp+ * 3 - stopped by itmax+ * 4 - singular matrix. Restart from current p with increased mu+ * 5 - no further error reduction is possible. Restart with increased mu+ * 6 - stopped by small ||e||_2+ * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error+ * info[7]= # function evaluations+ * info[8]= # Jacobian evaluations+ * info[9]= # linear systems solved, i.e. # attempts for reducing error+ */+ LM_REAL *work, /* working memory at least LM_LEC_DIF_WORKSZ() reals large, allocated if NULL */+ LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */+ void *adata) /* pointer to possibly additional data, passed uninterpreted to func.+ * Set to NULL if not needed+ */+{+ struct LMLEC_DATA data;+ LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */+ int mm, ret;+ register int i, j;+ register LM_REAL tmp;+ LM_REAL locinfo[LM_INFO_SZ];++ mm=m-k;++ if(n<mm){+ PRINT_ERROR(LCAT(LEVMAR_LEC_DIF, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k, m);+ return LM_ERROR_TOO_FEW_MEASUREMENTS;+ }++ ptr=(LM_REAL *)malloc((2*m + m*mm + mm)*sizeof(LM_REAL));+ if(!ptr){+ PRINT_ERROR(LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }+ data.p=p;+ p0=ptr;+ data.c=p0+m;+ data.Z=Z=data.c+m;+ data.jac=NULL;+ pp=data.Z+m*mm;+ data.ncnstr=k;+ data.func=func;+ data.jacf=NULL;+ data.adata=adata;++ ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z+ if(ret<0){+ free(ptr);+ return ret;+ }++ /* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)+ * Due to orthogonality, Z^T Z = I and the last equation+ * becomes pp=Z^T*(p-c). Also, save the starting p in p0+ */+ for(i=0; i<m; ++i){+ p0[i]=p[i];+ p[i]-=data.c[i];+ }++ /* Z^T*(p-c) */+ for(i=0; i<mm; ++i){+ for(j=0, tmp=0.0; j<m; ++j)+ tmp+=Z[j*mm+i]*p[j];+ pp[i]=tmp;+ }++ /* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */+ for(i=0; i<m; ++i){+ Zimm=Z+i*mm;+ for(j=0, tmp=data.c[i]; j<mm; ++j)+ tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];+ if(FABS(tmp-p0[i])>LM_CNST(1E-03))+ PRINT_ERROR(RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DIF) "()! [%.10g reset to %.10g]\n",+ i, p0[i], tmp);+ }++ if(!info) info=locinfo; /* make sure that LEVMAR_DIF() is called with non-null info */+ /* note that covariance computation is not requested from LEVMAR_DIF() */+ ret=LEVMAR_DIF(LMLEC_FUNC, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);++ /* p=c + Z*pp */+ for(i=0; i<m; ++i){+ Zimm=Z+i*mm;+ for(j=0, tmp=data.c[i]; j<mm; ++j)+ tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];+ p[i]=tmp;+ }++ /* compute the Jacobian with finite differences and use it to estimate the covariance */+ if(covar){+ LM_REAL *hx, *wrk, *jac;++ hx=(LM_REAL *)malloc((2*n+n*m)*sizeof(LM_REAL));+ if(!hx){+ PRINT_ERROR(LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));+ free(ptr);+ return LM_ERROR_MEMORY_ALLOCATION_FAILURE;+ }++ wrk=hx+n;+ jac=wrk+n;++ (*func)(p, hx, m, n, adata); /* evaluate function at p */+ LEVMAR_FDIF_FORW_JAC_APPROX(func, p, hx, wrk, (LM_REAL)LM_DIFF_DELTA, jac, m, n, adata); /* compute the Jacobian at p */+ LEVMAR_TRANS_MAT_MAT_MULT(jac, covar, n, m); /* covar = J^T J */+ LEVMAR_COVAR(covar, covar, info[1], m, n);+ free(hx);+ }++ free(ptr);++ return ret;+}++/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */+#undef LMLEC_DATA+#undef LMLEC_ELIM+#undef LMLEC_FUNC+#undef LMLEC_JACF+#undef LEVMAR_FDIF_FORW_JAC_APPROX+#undef LEVMAR_COVAR+#undef LEVMAR_TRANS_MAT_MAT_MULT+#undef LEVMAR_LEC_DER+#undef LEVMAR_LEC_DIF+#undef LEVMAR_DER+#undef LEVMAR_DIF++#undef GEQP3+#undef ORGQR+#undef TRTRI
+ levmar-2.4/matlab/CMakeLists.txt view
@@ -0,0 +1,58 @@+# CMake file for levmar's MEX-file; see http://www.cmake.org +# Requires FindMatlab.cmake included with cmake + +PROJECT(LEVMARMEX) +#CMAKE_MINIMUM_REQUIRED(VERSION 1.4) + +INCLUDE("C:/Program Files/CMake 2.4/share/cmake-2.4/Modules/FindMatlab.cmake") + +# f2c is sometimes equivalent to libF77 & libI77; in that case, set HAVE_F2C to 0 +SET(HAVE_F2C 1 CACHE BOOL "Do we have f2c or F77/I77?" ) + +# the directory where the lapack/blas/f2c libraries reside +SET(LAPACKBLAS_DIR /usr/lib CACHE PATH "Path to lapack/blas libraries") + +# the directory where lm.h resides +SET(LM_H_DIR .. CACHE PATH "Path to lm.h") +# the directory where the levmar library resides +SET(LEVMAR_DIR .. CACHE PATH "Path to levmar library") + +# actual names for the lapack/blas/f2c libraries +SET(LAPACK_LIB lapack CACHE STRING "The name of the lapack library") +SET(BLAS_LIB blas CACHE STRING "The name of the blas library") +IF(HAVE_F2C) + SET(F2C_LIB f2c CACHE STRING "The name of the f2c library") +ELSE(HAVE_F2C) + SET(F77_LIB libF77 CACHE STRING "The name of the F77 library") + SET(I77_LIB libI77 CACHE STRING "The name of the I77 library") +ENDIF(HAVE_F2C) + +########################## NO CHANGES BEYOND THIS POINT ########################## + +INCLUDE_DIRECTORIES(${LM_H_DIR}) +LINK_DIRECTORIES(${LAPACKBLAS_DIR} ${LEVMAR_DIR}) + +SET(SRC levmar.c) + +# naming conventions for the generated file's suffix +IF(WIN32) + SET(SUFFIX ".mexw32") +ELSE(WIN32) + SET(SUFFIX ".mexglx") +ENDIF(WIN32) + +SET(OUTNAME "levmar${SUFFIX}") + +ADD_LIBRARY(${OUTNAME} MODULE ${SRC}) + +IF(HAVE_F2C) + ADD_CUSTOM_COMMAND(OUTPUT ${OUTNAME} + DEPENDS ${SRC} + COMMAND mex + ARGS -O -I${LM_H_DIR} ${SRC} -I${MATLAB_INCLUDE_DIR} -L${LAPACKBLAS_DIR} -L${LEVMAR_DIR} -L${MATLAB_MEX_LIBRARY} -llevmar -l${LAPACK_LIB} -l${BLAS_LIB} -l${F2C_LIB} -output ${MATLAB_LIBRARIES} ${OUTNAME}) +ELSE(HAVE_F2C) + ADD_CUSTOM_COMMAND(OUTPUT ${OUTNAME} + DEPENDS ${SRC} + COMMAND mex + ARGS -O -I${LM_H_DIR} ${SRC} -I${MATLAB_INCLUDE_DIR} -L${LAPACKBLAS_DIR} -L${LEVMAR_DIR} -L${MATLAB_MEX_LIBRARY} -llevmar -l${LAPACK_LIB} -l${BLAS_LIB} -l${F77_LIB} -l${I77_LIB} ${MATLAB_LIBRARIES} -output ${OUTNAME}) +ENDIF(HAVE_F2C)
+ levmar-2.4/matlab/Makefile view
@@ -0,0 +1,30 @@+#+# Unix/Linux Makefile for MATLAB interface to levmar+#++MEX=mex+MEXCFLAGS=-I.. -O #-g+# WHEN USING LAPACK, CHANGE THE NEXT TWO LINES TO WHERE YOUR COMPILED LAPACK/BLAS & F2C LIBS ARE!+LAPACKBLASLIBS_PATH=/usr/lib+F2CLIBS_PATH=/usr/local/lib+++# I had to specify the absolute path to the libs, otherwise mex linked against their dynamic versions...+INTFACESRCS=levmar.c+LAPACKLIBS=$(LAPACKBLASLIBS_PATH)/liblapack.a $(LAPACKBLASLIBS_PATH)/libblas.a $(F2CLIBS_PATH)/libf2c.a+ # On systems with a FORTRAN (not f2c'ed) version of LAPACK, libf2c.a is+ # not necessary; on others, libf2c.a comes in two parts: libF77.a and libI77.a++LIBS=$(LAPACKLIBS)++dummy: $(INTFACESRCS)+ $(MEX) $(MEXCFLAGS) $(INTFACESRCS) ../liblevmar.a $(LIBS)++clean:+ @rm -f levmar.mexglx++depend:+ makedepend -f Makefile $(INTFACESRCS)++# DO NOT DELETE THIS LINE -- make depend depends on it.+
+ levmar-2.4/matlab/Makefile.w32 view
@@ -0,0 +1,26 @@+#+# Windows Makefile for MATLAB interface to levmar+#++MEX=mex+MEXCFLAGS=-I.. -O #-g+# WHEN USING LAPACK, CHANGE THE NEXT TWO LINES TO WHERE YOUR COMPILED LAPACK/BLAS & F2C LIBS ARE!+LAPACKBLASLIBS_PATH=C:\src\lib+F2CLIBS_PATH=$(LAPACKBLASLIBS_PATH) # define appropriately if not identical to LAPACKBLASLIBS_PATH+++INTFACESRCS=levmar.c+LAPACKLIBS=$(LAPACKBLASLIBS_PATH)/clapack.lib $(LAPACKBLASLIBS_PATH)/blas.lib $(F2CLIBS_PATH)/libF77.lib $(F2CLIBS_PATH)/libI77.lib+LIBS=$(LAPACKLIBS)++dummy: $(INTFACESRCS)+ $(MEX) $(MEXCFLAGS) $(INTFACESRCS) ../levmar.lib $(LIBS)++clean:+ -del levmar.mexw32++depend:+ makedepend -f Makefile $(INTFACESRCS)++# DO NOT DELETE THIS LINE -- make depend depends on it.+
+ levmar-2.4/matlab/README.txt view
@@ -0,0 +1,35 @@+This directory contains a matlab MEX interface to levmar. This interface +has been tested with Matlab v. 6.5 R13 under linux and v. 7.4 R2007 under Windows. +Users have also reported success with Octave. + +FILES +The following files are included: +levmar.c: C MEX-file for levmar +Makefile: UNIX makefile for compiling levmar.c using mex +Makefile.w32: Windows makefile for compiling levmar.c using mex +levmar.m: Documentation for the MEX interface +lmdemo.m: Demonstration of using the MEX interface; run as matlab < lmdemo.m + +*.m: Matlab functions implementing various objective functions and their Jacobians. + For instance, meyer.m implements the objective function for Meyer's (reformulated) + problem and jacmeyer.m implements its Jacobian. + + + +COMPILING +Use the provided Makefile or Makefile.w32, depending on your platform. +Alternatively, levmar.c can be compiled from matlab's prompt with a +command like + +mex -DHAVE_LAPACK -I.. -O -L<levmar library dir> -L<blas/lapack libraries dir> levmar.c -llevmar -lclapack -lblas -lf2c + +Make sure that you substitute the angle brackets with the correct paths to +the levmar and the blas/lapack directories. Also, on certain systems, +-lf2c should be changed to -llibF77 -llibI77 +If your mex compiler has not been configured, the following command should be run first: + +mex -setup + + +TESTING +After compiling, execute lmdemo.m with matlab < lmdemo.m
+ levmar-2.4/matlab/bt3.m view
@@ -0,0 +1,11 @@+function x = bt3(p, adata)+ n=5;++ t1=p(1)-p(2);+ t2=p(2)+p(3)-2.0;+ t3=p(4)-1.0;+ t4=p(5)-1.0;++ for i=1:n+ x(i)=t1*t1 + t2*t2 + t3*t3 + t4*t4;+ end
+ levmar-2.4/matlab/expfit.m view
@@ -0,0 +1,8 @@+function x = expfit(p, data)+ n=data;++% data1, data2 are actually unused++ for i=1:n+ x(i)=p(1)*exp(-p(2)*i) + p(3);+ end
+ levmar-2.4/matlab/hs01.m view
@@ -0,0 +1,6 @@+function x = hs01(p)+ n=2;++ t=p(1)*p(1);+ x(1)=10.0*(p(2)-t);+ x(2)=1.0-p(1);
+ levmar-2.4/matlab/jacbt3.m view
@@ -0,0 +1,13 @@+function jac = jacbt3(p, adata)+ n=5;+ m=5;++ t1=p(1)-p(2);+ t2=p(2)+p(3)-2.0;+ t3=p(4)-1.0;+ t4=p(5)-1.0;++ for i=1:n+ jac(i, 1:m)=[2.0*t1, 2.0*(t2-t1), 2.0*t2, 2.0*t3, 2.0*t4];+ end+
+ levmar-2.4/matlab/jacexpfit.m view
@@ -0,0 +1,7 @@+function jac = jacexpfit(p, data)+ n=data;+ m=max(size(p));++ for i=1:n+ jac(i, 1:m)=[exp(-p(2)*i), -p(1)*i*exp(-p(2)*i), 1.0];+ end
+ levmar-2.4/matlab/jachs01.m view
@@ -0,0 +1,5 @@+function jac = jachs01(p)+ m=2;++ jac(1, 1:m)=[-20.0*p(1), 10.0];+ jac(2, 1:m)=[-1.0, 0.0];
+ levmar-2.4/matlab/jacmeyer.m view
@@ -0,0 +1,10 @@+function jac = jacmeyer(p, data1, data2)+ n=16;+ m=3;++ for i=1:n+ ui=0.45+0.05*i;+ tmp=exp(10.0*p(2)/(ui+p(3)) - 13.0);++ jac(i, 1:m)=[tmp, 10.0*p(1)*tmp/(ui+p(3)), -10.0*p(1)*p(2)*tmp/((ui+p(3))*(ui+p(3)))];+ end
+ levmar-2.4/matlab/jacmodhs52.m view
@@ -0,0 +1,7 @@+function jac = jacmodhs52(p)+ m=5;++ jac(1, 1:m)=[4.0, -1.0, 0.0, 0.0, 0.0];+ jac(2, 1:m)=[0.0, 1.0, 1.0, 0.0, 0.0];+ jac(3, 1:m)=[0.0, 0.0, 0.0, 1.0, 0.0];+ jac(4, 1:m)=[0.0, 0.0, 0.0, 0.0, 1.0];
+ levmar-2.4/matlab/levmar.c view
@@ -0,0 +1,582 @@+/* ////////////////////////////////////////////////////////////////////////////////+//+// Matlab MEX file for the Levenberg - Marquardt minimization algorithm+// Copyright (C) 2007 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+//////////////////////////////////////////////////////////////////////////////// */++#include <stdio.h>+#include <stdlib.h>+#include <stdarg.h>+#include <math.h>+#include <string.h>+#include <ctype.h>++#include <lm.h>++#include <mex.h>++/**+#define DEBUG+**/++#ifndef HAVE_LAPACK+#ifdef _MSC_VER+#pragma message("LAPACK not available, certain functionalities cannot be compiled!")+#else+#warning LAPACK not available, certain functionalities cannot be compiled+#endif /* _MSC_VER */+#endif /* HAVE_LAPACK */++#define __MAX__(A, B) ((A)>=(B)? (A) : (B))++#define MIN_UNCONSTRAINED 0+#define MIN_CONSTRAINED_BC 1+#define MIN_CONSTRAINED_LEC 2+#define MIN_CONSTRAINED_BLEC 3++#define ERROR_FAILED_FUNC_AND_JACOBIAN_CHECK -1++struct mexdata {+ /* matlab names of the fitting function & its Jacobian */+ char *fname, *jacname;++ /* binary flags specifying if input p0 is a row or column vector */+ int isrow_p0;++ /* rhs args to be passed to matlab. rhs[0] is reserved for+ * passing the parameter vector. If present, problem-specific+ * data are passed in rhs[1], rhs[2], etc+ */+ mxArray **rhs;+ int nrhs; /* >= 1 */+};++/* display printf-style error messages in matlab */+static void matlabFmtdErrMsgTxt(char *fmt, ...)+{+char buf[256];+va_list args;++ va_start(args, fmt);+ vsprintf(buf, fmt, args);+ va_end(args);++ mexErrMsgTxt(buf);+}++/* display printf-style warning messages in matlab */+static void matlabFmtdWarnMsgTxt(char *fmt, ...)+{+char buf[256];+va_list args;++ va_start(args, fmt);+ vsprintf(buf, fmt, args);+ va_end(args);++ mexWarnMsgTxt(buf);+}++static void func(double *p, double *hx, int m, int n, void *adata)+{+mxArray *lhs[1];+double *mp, *mx;+register int i;+struct mexdata *dat=(struct mexdata *)adata;++ /* prepare to call matlab */+ mp=mxGetPr(dat->rhs[0]);+ for(i=0; i<m; ++i)+ mp[i]=p[i];++ /* invoke matlab */+ mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->fname);++ /* copy back results & cleanup */+ mx=mxGetPr(lhs[0]);+ for(i=0; i<n; ++i)+ hx[i]=mx[i];++ /* delete the matrix created by matlab */+ mxDestroyArray(lhs[0]);+}++static void jacfunc(double *p, double *j, int m, int n, void *adata)+{+mxArray *lhs[1];+double *mp;+double *mj;+register int i, k;+struct mexdata *dat=(struct mexdata *)adata;++ /* prepare to call matlab */+ mp=mxGetPr(dat->rhs[0]);+ for(i=0; i<m; ++i)+ mp[i]=p[i];++ /* invoke matlab */+ mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->jacname);++ /* copy back results & cleanup. Note that the nxm Jacobian+ * computed by matlab should be transposed so that+ * levmar gets it in row major, as expected+ */+ mj=mxGetPr(lhs[0]);+ for(i=0; i<n; ++i)+ for(k=0; k<m; ++k)+ j[i*m+k]=mj[i+k*n];++ /* delete the matrix created by matlab */+ mxDestroyArray(lhs[0]);+}++/* matlab matrices are in column-major, this routine converts them to row major for levmar */+static double *getTranspose(mxArray *Am)+{+int m, n;+double *At, *A;+register int i, j;++ m=mxGetM(Am);+ n=mxGetN(Am);+ A=mxGetPr(Am);+ At=mxMalloc(m*n*sizeof(double));++ for(i=0; i<m; i++)+ for(j=0; j<n; j++)+ At[i*n+j]=A[i+j*m];++ return At;+}++/* check the supplied matlab function and its Jacobian. Returns 1 on error, 0 otherwise */+static int checkFuncAndJacobian(double *p, int m, int n, int havejac, struct mexdata *dat)+{+mxArray *lhs[1];+register int i;+int ret=0;+double *mp;++ mexSetTrapFlag(1); /* handle errors in the MEX-file */++ mp=mxGetPr(dat->rhs[0]);+ for(i=0; i<m; ++i)+ mp[i]=p[i];++ /* attempt to call the supplied func */+ i=mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->fname);+ if(i){+ PRINT_ERROR("levmar: error calling '%s'.\n", dat->fname);+ ret=1;+ }+ else if(!mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) || !(mxGetM(lhs[0])==1 || mxGetN(lhs[0])==1) ||+ __MAX__(mxGetM(lhs[0]), mxGetN(lhs[0]))!=n){+ PRINT_ERROR("levmar: '%s' should produce a real vector with %d elements (got %d).\n",+ dat->fname, n, __MAX__(mxGetM(lhs[0]), mxGetN(lhs[0])));+ ret=1;+ }+ /* delete the matrix created by matlab */+ mxDestroyArray(lhs[0]);++ if(havejac){+ /* attempt to call the supplied jac */+ i=mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->jacname);+ if(i){+ PRINT_ERROR("levmar: error calling '%s'.\n", dat->jacname);+ ret=1;+ }+ else if(!mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) || mxGetM(lhs[0])!=n || mxGetN(lhs[0])!=m){+ PRINT_ERROR("levmar: '%s' should produce a real %dx%d matrix (got %dx%d).\n",+ dat->jacname, n, m, mxGetM(lhs[0]), mxGetN(lhs[0]));+ ret=1;+ }+ else if(mxIsSparse(lhs[0])){+ PRINT_ERROR("levmar: '%s' should produce a real dense matrix (got a sparse one).\n", dat->jacname);+ ret=1;+ }+ /* delete the matrix created by matlab */+ mxDestroyArray(lhs[0]);+ }++ mexSetTrapFlag(0); /* on error terminate the MEX-file and return control to the MATLAB prompt */++ return ret;+}+++/*+[ret, p, info, covar]=levmar_der (f, j, p0, x, itmax, opts, 'unc' ...)+[ret, p, info, covar]=levmar_bc (f, j, p0, x, itmax, opts, 'bc', lb, ub, ...)+[ret, p, info, covar]=levmar_lec (f, j, p0, x, itmax, opts, 'lec', A, b, ...)+[ret, p, info, covar]=levmar_blec(f, j, p0, x, itmax, opts, 'blec', lb, ub, A, b, wghts, ...)+*/++void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *Prhs[])+{+register int i;+register double *pdbl;+mxArray **prhs=(mxArray **)&Prhs[0], *At;+struct mexdata mdata;+int len, status;+double *p, *p0, *ret, *x;+int m, n, havejac, Arows, itmax, nopts, mintype, nextra;+double opts[LM_OPTS_SZ]={LM_INIT_MU, LM_STOP_THRESH, LM_STOP_THRESH, LM_STOP_THRESH, LM_DIFF_DELTA};+double info[LM_INFO_SZ];+double *lb=NULL, *ub=NULL, *A=NULL, *b=NULL, *wghts=NULL, *covar=NULL;++ /* parse input args; start by checking their number */+ if((nrhs<5))+ matlabFmtdErrMsgTxt("levmar: at least 5 input arguments required (got %d).", nrhs);+ if(nlhs>4)+ matlabFmtdErrMsgTxt("levmar: too many output arguments (max. 4, got %d).", nlhs);+ else if(nlhs<2)+ matlabFmtdErrMsgTxt("levmar: too few output arguments (min. 2, got %d).", nlhs);++ /* note that in order to accommodate optional args, prhs & nrhs are adjusted accordingly below */++ /** func **/+ /* first argument must be a string , i.e. a char row vector */+ if(mxIsChar(prhs[0])!=1)+ mexErrMsgTxt("levmar: first argument must be a string.");+ if(mxGetM(prhs[0])!=1)+ mexErrMsgTxt("levmar: first argument must be a string (i.e. char row vector).");+ /* store supplied name */+ len=mxGetN(prhs[0])+1;+ mdata.fname=mxCalloc(len, sizeof(char));+ status=mxGetString(prhs[0], mdata.fname, len);+ if(status!=0)+ mexErrMsgTxt("levmar: not enough space. String is truncated.");++ /** jac (optional) **/+ /* check whether second argument is a string */+ if(mxIsChar(prhs[1])==1){+ if(mxGetM(prhs[1])!=1)+ mexErrMsgTxt("levmar: second argument must be a string (i.e. row vector).");+ /* store supplied name */+ len=mxGetN(prhs[1])+1;+ mdata.jacname=mxCalloc(len, sizeof(char));+ status=mxGetString(prhs[1], mdata.jacname, len);+ if(status!=0)+ mexErrMsgTxt("levmar: not enough space. String is truncated.");+ havejac=1;++ ++prhs;+ --nrhs;+ }+ else{+ mdata.jacname=NULL;+ havejac=0;+ }++#ifdef DEBUG+ fflush(stderr);+ PRINT_ERROR("LEVMAR: %s analytic Jacobian\n", havejac? "with" : "no");+#endif /* DEBUG */++/* CHECK+if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 && mxGetN(prhs[1])==1))+*/++ /** p0 **/+ /* the second required argument must be a real row or column vector */+ if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 || mxGetN(prhs[1])==1))+ mexErrMsgTxt("levmar: p0 must be a real vector.");+ p0=mxGetPr(prhs[1]);+ /* determine if we have a row or column vector and retrieve its+ * size, i.e. the number of parameters+ */+ if(mxGetM(prhs[1])==1){+ m=mxGetN(prhs[1]);+ mdata.isrow_p0=1;+ }+ else{+ m=mxGetM(prhs[1]);+ mdata.isrow_p0=0;+ }+ /* copy input parameter vector to avoid destroying it */+ p=mxMalloc(m*sizeof(double));+ for(i=0; i<m; ++i)+ p[i]=p0[i];++ /** x **/+ /* the third required argument must be a real row or column vector */+ if(!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) || !(mxGetM(prhs[2])==1 || mxGetN(prhs[2])==1))+ mexErrMsgTxt("levmar: x must be a real vector.");+ x=mxGetPr(prhs[2]);+ n=__MAX__(mxGetM(prhs[2]), mxGetN(prhs[2]));++ /** itmax **/+ /* the fourth required argument must be a scalar */+ if(!mxIsDouble(prhs[3]) || mxIsComplex(prhs[3]) || mxGetM(prhs[3])!=1 || mxGetN(prhs[3])!=1)+ mexErrMsgTxt("levmar: itmax must be a scalar.");+ itmax=(int)mxGetScalar(prhs[3]);++ /** opts **/+ /* the fifth required argument must be a real row or column vector */+ if(!mxIsDouble(prhs[4]) || mxIsComplex(prhs[4]) || (!(mxGetM(prhs[4])==1 || mxGetN(prhs[4])==1) &&+ !(mxGetM(prhs[4])==0 && mxGetN(prhs[4])==0)))+ mexErrMsgTxt("levmar: opts must be a real vector.");+ pdbl=mxGetPr(prhs[4]);+ nopts=__MAX__(mxGetM(prhs[4]), mxGetN(prhs[4]));+ if(nopts!=0){ /* if opts==[], nothing needs to be done and the defaults are used */+ if(nopts>LM_OPTS_SZ)+ matlabFmtdErrMsgTxt("levmar: opts must have at most %d elements, got %d.", LM_OPTS_SZ, nopts);+ else if(nopts<((havejac)? LM_OPTS_SZ-1 : LM_OPTS_SZ))+ matlabFmtdWarnMsgTxt("levmar: only the %d first elements of opts specified, remaining set to defaults.", nopts);+ for(i=0; i<nopts; ++i)+ opts[i]=pdbl[i];+ }+#ifdef DEBUG+ else{+ fflush(stderr);+ PRINT_ERROR("LEVMAR: empty options vector, using defaults\n");+ }+#endif /* DEBUG */++ /** mintype (optional) **/+ /* check whether sixth argument is a string */+ if(nrhs>=6 && mxIsChar(prhs[5])==1 && mxGetM(prhs[5])==1){+ char *minhowto;++ /* examine supplied name */+ len=mxGetN(prhs[5])+1;+ minhowto=mxCalloc(len, sizeof(char));+ status=mxGetString(prhs[5], minhowto, len);+ if(status!=0)+ mexErrMsgTxt("levmar: not enough space. String is truncated.");++ for(i=0; minhowto[i]; ++i)+ minhowto[i]=tolower(minhowto[i]);+ if(!strncmp(minhowto, "unc", 3)) mintype=MIN_UNCONSTRAINED;+ else if(!strncmp(minhowto, "bc", 2)) mintype=MIN_CONSTRAINED_BC;+ else if(!strncmp(minhowto, "lec", 3)) mintype=MIN_CONSTRAINED_LEC;+ else if(!strncmp(minhowto, "blec", 4)) mintype=MIN_CONSTRAINED_BLEC;+ else matlabFmtdErrMsgTxt("levmar: unknown minimization type '%s'.", minhowto);++ mxFree(minhowto);++ ++prhs;+ --nrhs;+ }+ else+ mintype=MIN_UNCONSTRAINED;++ if(mintype==MIN_UNCONSTRAINED) goto extraargs;++ /* arguments below this point are optional and their presence depends+ * upon the minimization type determined above+ */+ /** lb, ub **/+ if(nrhs>=7 && (mintype==MIN_CONSTRAINED_BC || mintype==MIN_CONSTRAINED_BLEC)){+ /* check if the next two arguments are real row or column vectors */+ if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){+ if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){+ if((i=__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5])))!=m)+ matlabFmtdErrMsgTxt("levmar: lb must have %d elements, got %d.", m, i);+ if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=m)+ matlabFmtdErrMsgTxt("levmar: ub must have %d elements, got %d.", m, i);++ lb=mxGetPr(prhs[5]);+ ub=mxGetPr(prhs[6]);++ prhs+=2;+ nrhs-=2;+ }+ }+ }++ /** A, b **/+ if(nrhs>=7 && (mintype==MIN_CONSTRAINED_LEC || mintype==MIN_CONSTRAINED_BLEC)){+ /* check if the next two arguments are a real matrix and a real row or column vector */+ if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && mxGetM(prhs[5])>=1 && mxGetN(prhs[5])>=1){+ if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){+ if((i=mxGetN(prhs[5]))!=m)+ matlabFmtdErrMsgTxt("levmar: A must have %d columns, got %d.", m, i);+ if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=(Arows=mxGetM(prhs[5])))+ matlabFmtdErrMsgTxt("levmar: b must have %d elements, got %d.", Arows, i);++ At=prhs[5];+ b=mxGetPr(prhs[6]);+ A=getTranspose(At);++ prhs+=2;+ nrhs-=2;+ }+ }+ }++ /* wghts */+ /* check if we have a weights vector */+ if(nrhs>=6 && mintype==MIN_CONSTRAINED_BLEC){ /* only check if we have seen both box & linear constraints */+ if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){+ if(__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5]))==m){+ wghts=mxGetPr(prhs[5]);++ ++prhs;+ --nrhs;+ }+ }+ }+ /* arguments below this point are assumed to be extra arguments passed+ * to every invocation of the fitting function and its Jacobian+ */++extraargs:+ /* handle any extra args and allocate memory for+ * passing the current parameter estimate to matlab+ */+ nextra=nrhs-5;+ mdata.nrhs=nextra+1;+ mdata.rhs=(mxArray **)mxMalloc(mdata.nrhs*sizeof(mxArray *));+ for(i=0; i<nextra; ++i)+ mdata.rhs[i+1]=(mxArray *)prhs[nrhs-nextra+i]; /* discard 'const' modifier */+#ifdef DEBUG+ fflush(stderr);+ PRINT_ERROR("LEVMAR: %d extra args\n", nextra);+#endif /* DEBUG */++ if(mdata.isrow_p0){ /* row vector */+ mdata.rhs[0]=mxCreateDoubleMatrix(1, m, mxREAL);+ /*+ mxSetM(mdata.rhs[0], 1);+ mxSetN(mdata.rhs[0], m);+ */+ }+ else{ /* column vector */+ mdata.rhs[0]=mxCreateDoubleMatrix(m, 1, mxREAL);+ /*+ mxSetM(mdata.rhs[0], m);+ mxSetN(mdata.rhs[0], 1);+ */+ }++ /* ensure that the supplied function & Jacobian are as expected */+ if(checkFuncAndJacobian(p, m, n, havejac, &mdata)){+ status=ERROR_FAILED_FUNC_AND_JACOBIAN_CHECK;+ goto cleanup;+ }++ if(nlhs>3) /* covariance output required */+ covar=mxMalloc(m*m*sizeof(double));++ /* invoke levmar */+ if(!lb && !ub){+ if(!A && !b){ /* no constraints */+ if(havejac)+ status=dlevmar_der(func, jacfunc, p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);+ else+ status=dlevmar_dif(func, p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);+#ifdef DEBUG+ fflush(stderr);+ PRINT_ERROR("LEVMAR: calling dlevmar_der()/dlevmar_dif()\n");+#endif /* DEBUG */+ }+ else{ /* linear constraints */+#ifdef HAVE_LAPACK+ if(havejac)+ status=dlevmar_lec_der(func, jacfunc, p, x, m, n, A, b, Arows, itmax, opts, info, NULL, covar, (void *)&mdata);+ else+ status=dlevmar_lec_dif(func, p, x, m, n, A, b, Arows, itmax, opts, info, NULL, covar, (void *)&mdata);+#else+ mexErrMsgTxt("levmar: no linear constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");+#endif /* HAVE_LAPACK */++#ifdef DEBUG+ fflush(stderr);+ PRINT_ERROR("LEVMAR: calling dlevmar_lec_der()/dlevmar_lec_dif()\n");+#endif /* DEBUG */+ }+ }+ else{+ if(!A && !b){ /* box constraints */+ if(havejac)+ status=dlevmar_bc_der(func, jacfunc, p, x, m, n, lb, ub, itmax, opts, info, NULL, covar, (void *)&mdata);+ else+ status=dlevmar_bc_dif(func, p, x, m, n, lb, ub, itmax, opts, info, NULL, covar, (void *)&mdata);+#ifdef DEBUG+ fflush(stderr);+ PRINT_ERROR("LEVMAR: calling dlevmar_bc_der()/dlevmar_bc_dif()\n");+#endif /* DEBUG */+ }+ else{ /* box & linear constraints */+#ifdef HAVE_LAPACK+ if(havejac)+ status=dlevmar_blec_der(func, jacfunc, p, x, m, n, lb, ub, A, b, Arows, wghts, itmax, opts, info, NULL, covar, (void *)&mdata);+ else+ status=dlevmar_blec_dif(func, p, x, m, n, lb, ub, A, b, Arows, wghts, itmax, opts, info, NULL, covar, (void *)&mdata);+#else+ mexErrMsgTxt("levmar: no box & linear constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");+#endif /* HAVE_LAPACK */++#ifdef DEBUG+ fflush(stderr);+ PRINT_ERROR("LEVMAR: calling dlevmar_blec_der()/dlevmar_blec_dif()\n");+#endif /* DEBUG */+ }+ }+#ifdef DEBUG+ fflush(stderr);+ printf("LEVMAR: minimization returned %d in %g iter, reason %g\n\tSolution: ", status, info[5], info[6]);+ for(i=0; i<m; ++i)+ printf("%.7g ", p[i]);+ printf("\n\n\tMinimization info:\n\t");+ for(i=0; i<LM_INFO_SZ; ++i)+ printf("%g ", info[i]);+ printf("\n");+#endif /* DEBUG */++ /* copy back return results */+ /** ret **/+ plhs[0]=mxCreateDoubleMatrix(1, 1, mxREAL);+ ret=mxGetPr(plhs[0]);+ ret[0]=(double)status;++ /** popt **/+ plhs[1]=(mdata.isrow_p0==1)? mxCreateDoubleMatrix(1, m, mxREAL) : mxCreateDoubleMatrix(m, 1, mxREAL);+ pdbl=mxGetPr(plhs[1]);+ for(i=0; i<m; ++i)+ pdbl[i]=p[i];++ /** info **/+ if(nlhs>2){+ plhs[2]=mxCreateDoubleMatrix(1, LM_INFO_SZ, mxREAL);+ pdbl=mxGetPr(plhs[2]);+ for(i=0; i<LM_INFO_SZ; ++i)+ pdbl[i]=info[i];+ }++ /** covar **/+ if(nlhs>3){+ plhs[3]=mxCreateDoubleMatrix(m, m, mxREAL);+ pdbl=mxGetPr(plhs[3]);+ for(i=0; i<m*m; ++i) /* covariance matrices are symmetric, thus no need to transpose! */+ pdbl[i]=covar[i];+ }++cleanup:+ /* cleanup */+ mxDestroyArray(mdata.rhs[0]);+ if(A) mxFree(A);++ mxFree(mdata.fname);+ if(havejac) mxFree(mdata.jacname);+ mxFree(p);+ mxFree(mdata.rhs);+ if(covar) mxFree(covar);++ if(status<0)+ mexWarnMsgTxt("levmar: optimization returned with an error!");+}
+ levmar-2.4/matlab/levmar.m view
@@ -0,0 +1,71 @@+function [ret, popt, info, covar]=levmar(fname, jacname, p0, x, itmax, opts, type)+% LEVMAR matlab MEX interface to the levmar non-linear least squares minimization+% library available from http://www.ics.forth.gr/~lourakis/levmar/+% +% levmar can be used in any of the 4 following ways:+% [ret, popt, info, covar]=levmar(fname, jacname, p0, x, itmax, opts, 'unc', ...)+% [ret, popt, info, covar]=levmar(fname, jacname, p0, x, itmax, opts, 'bc', lb, ub, ...)+% [ret, popt, info, covar]=levmar(fname, jacname, p0, x, itmax, opts, 'lec', A, b, ...)+% [ret, popt, info, covar]=levmar(fname, jacname, p0, x, itmax, opts, 'blec', lb, ub, A, b, wghts, ...)+% +% The dots at the end denote any additional, problem specific data that are passed uniterpreted to+% all invocations of fname and jacname, see below for details.+%+% In the following, the word "vector" is meant to imply either a row or a column vector.+%+% required input arguments:+% - fname: String defining the name of a matlab function implementing the function to be minimized.+% fname will be called as fname(p, ...), where p denotes the parameter vector and the dots any+% additional data passed as extra arguments during the invocation of levmar (refer to Meyer's+% problem in lmdemo.m for an example).+%+% - p0: vector of doubles holding the initial parameters estimates.+%+% - x: vector of doubles holding the measurements vector.+%+% - itmax: maximum number of iterations.+%+% - opts: vector of doubles specifying the minimization parameters, as follows:+% opts(1) scale factor for the initial damping factor+% opts(2) stopping threshold for ||J^T e||_inf+% opts(3) stopping threshold for ||Dp||_2+% opts(4) stopping threshold for ||e||_2+% opts(5) step used in finite difference approximation to the Jacobian.+% If an empty vector (i.e. []) is specified, defaults are used.+% +% optional input arguments:+% - jacname: String defining the name of matlab function implementing the Jacobian of function fname.+% jacname will be called as jacname(p, ...) where p is again the parameter vector and the dots+% denote any additional data passed as extra arguments to the invocation of levmar. If omitted,+% the Jacobian is approximated with finite differences through repeated invocations of fname.+%+% - type: String defining the minimization type. It should be one of the following:+% 'unc' specifies unconstrained minimization.+% 'bc' specifies minimization subject to box constraints.+% 'lec' specifies minimization subject to linear equation constraints.+% 'blec' specifies minimization subject to box and linear equation constraints.+% If omitted, a default of 'unc' is assumed. Depending on the minimization type, the MEX+% interface will invoke one of dlevmar_XXX, dlevmar_bc_XXX, dlevmar_lec_XXX or dlevmar_blec_XXX+%+% - lb, ub: vectors of doubles specifying lower and upper bounds for p, respectively+%+% - A, b: k x m matrix and k vector specifying linear equation constraints for p, i.e. A*p=b+% A should have full rank.+%+% - wghts: vector of doubles specifying the weights for the penalty terms corresponding to+% the box constraints, see lmblec_core.c for more details. If omitted and a 'blec' type+% minimization is to be carried out, default weights are used.+% +%+% output arguments+% - ret: return value of levmar, corresponding to the number of iterations if successful, -1 otherwise.+%+% - popt: estimated minimizer, i.e. minimized parameters vector.+%+% - info: optional array of doubles, which upon return provides information regarding the minimization.+% See lm_core.c for more details.+%+% - covar: optional covariance matrix corresponding to the estimated minimizer.+%+ +error('levmar.m is used only for providing documentation to levmar; make sure that levmar.c has been compiled using mex');
+ levmar-2.4/matlab/lmdemo.m view
@@ -0,0 +1,106 @@+% Demo program for levmar's MEX-file interface+% Performs minimization of several test problems++% Unconstrained minimization++% fitting the exponential model x_i=p(1)*exp(-p(2)*i)+p(3) of expfit.c to noisy measurements obtained with (5.0 0.1 1.0)+p0=[1.0, 0.0, 0.0];+x=[5.8728, 5.4948, 5.0081, 4.5929, 4.3574, 4.1198, 3.6843, 3.3642, 2.9742, 3.0237, 2.7002, 2.8781,...+ 2.5144, 2.4432, 2.2894, 2.0938, 1.9265, 2.1271, 1.8387, 1.7791, 1.6686, 1.6232, 1.571, 1.6057,...+ 1.3825, 1.5087, 1.3624, 1.4206, 1.2097, 1.3129, 1.131, 1.306, 1.2008, 1.3469, 1.1837, 1.2102,...+ 0.96518, 1.2129, 1.2003, 1.0743];++options=[1E-03, 1E-15, 1E-15, 1E-20, 1E-06];+% arg demonstrates additional data passing to expfit/jacexpfit+arg=[40];++[ret, popt, info]=levmar('expfit', 'jacexpfit', p0, x, 200, options, arg);+disp('Exponential model fitting (see also ../expfit.c)');+popt+++% Meyer's (reformulated) problem+p0=[8.85, 4.0, 2.5];++x=[];+x(1:4)=[34.780, 28.610, 23.650, 19.630];+x(5:8)=[16.370, 13.720, 11.540, 9.744];+x(9:12)=[8.261, 7.030, 6.005, 5.147];+x(13:16)=[4.427, 3.820, 3.307, 2.872];++options=[1E-03, 1E-15, 1E-15, 1E-20, 1E-06];+% arg1, arg2 demonstrate additional dummy data passing to meyer/jacmeyer+arg1=[17];+arg2=[27];++%[ret, popt, info]=levmar('meyer', 'jacmeyer', p0, x, 200, options, arg1, arg2);++%[ret, popt, info, covar]=levmar('meyer', 'jacmeyer', p0, x, 200, options, arg1, arg2);+[ret, popt, info, covar]=levmar('meyer', p0, x, 200, options, 'unc', arg1, arg2);+disp('Meyer''s (reformulated) problem');+popt+++% Linear constraints++% Boggs-Tolle problem 3+p0=[2.0, 2.0, 2.0, 2.0, 2.0];+x=[0.0, 0.0, 0.0, 0.0, 0.0];+options=[1E-03, 1E-15, 1E-15, 1E-20];+adata=[];++A=[1.0, 3.0, 0.0, 0.0, 0.0;+ 0.0, 0.0, 1.0, 1.0, -2.0;+ 0.0, 1.0, 0.0, 0.0, -1.0];+b=[0.0, 0.0, 0.0]';++[ret, popt, info, covar]=levmar('bt3', 'jacbt3', p0, x, 200, options, 'lec', A, b, adata);+disp('Boggs-Tolle problem 3');+popt+++% Box constraints++% Hock-Schittkowski problem 01+p0=[-2.0, 1.0];+x=[0.0, 0.0];+lb=[-realmax, -1.5];+ub=[realmax, realmax];+options=[1E-03, 1E-15, 1E-15, 1E-20];++[ret, popt, info, covar]=levmar('hs01', 'jachs01', p0, x, 200, options, 'bc', lb, ub);+disp('Hock-Schittkowski problem 01');+popt+++% Box and linear constraints++% Hock-Schittkowski modified problem 52+p0=[2.0, 2.0, 2.0, 2.0, 2.0];+x=[0.0, 0.0, 0.0, 0.0];+lb=[-0.09, 0.0, -realmax, -0.2, 0.0];+ub=[realmax, 0.3, 0.25, 0.3, 0.3];+A=[1.0, 3.0, 0.0, 0.0, 0.0;+ 0.0, 0.0, 1.0, 1.0, -2.0;+ 0.0, 1.0, 0.0, 0.0, -1.0];+b=[0.0, 0.0, 0.0]';+options=[1E-03, 1E-15, 1E-15, 1E-20];++[ret, popt, info, covar]=levmar('modhs52', 'jacmodhs52', p0, x, 200, options, 'blec', lb, ub, A, b);+disp('Hock-Schittkowski modified problem 52');+popt++% Hock-Schittkowski modified problem 235+p0=[-2.0, 3.0, 1.0];+x=[0.0, 0.0];+lb=[-realmax, 0.1, 0.7];+ub=[realmax, 2.9, realmax];+A=[1.0, 0.0, 1.0;+ 0.0, 1.0, -4.0];+b=[-1.0, 0.0]';+options=[1E-03, 1E-15, 1E-15, 1E-20];++[ret, popt, info, covar]=levmar('mods235', p0, x, 200, options, 'blec', lb, ub, A, b);+disp('Hock-Schittkowski modified problem 235');+popt+
+ levmar-2.4/matlab/meyer.m view
@@ -0,0 +1,9 @@+function x = meyer(p, data1, data2)+ n=16;++% data1, data2 are actually unused++ for i=1:n+ ui=0.45+0.05*i;+ x(i)=p(1)*exp(10.0*p(2)/(ui+p(3)) - 13.0);+ end
+ levmar-2.4/matlab/modhs52.m view
@@ -0,0 +1,7 @@+function x = modhs52(p)+ n=4;++ x(1)=4.0*p(1)-p(2);+ x(2)=p(2)+p(3)-2.0;+ x(3)=p(4)-1.0;+ x(4)=p(5)-1.0;
+ levmar-2.4/matlab/mods235.m view
@@ -0,0 +1,5 @@+function x = mods235(p)+ n=2;++ x(1)=0.1*(p(1)-1.0);+ x(2)=p(2)-p(1)*p(1);
+ levmar-2.4/misc.c view
@@ -0,0 +1,70 @@+/////////////////////////////////////////////////////////////////////////////////+// +// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++/******************************************************************************** + * Miscelaneous functions for Levenberg-Marquardt nonlinear minimization. The+ * same core code is used with appropriate #defines to derive single and double+ * precision versions, see also misc_core.c+ ********************************************************************************/++#include <stdio.h>+#include <stdlib.h>+#include <math.h>+#include <float.h>++#include "lm.h"+#include "misc.h"++#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!+#endif++#ifdef LM_SNGL_PREC+/* single precision (float) definitions */+#define LM_REAL float+#define LM_PREFIX s++#define LM_REAL_EPSILON FLT_EPSILON+#define __SUBCNST(x) x##F+#define LM_CNST(x) __SUBCNST(x) // force substitution++#include "misc_core.c" // read in core code++#undef LM_REAL+#undef LM_PREFIX+#undef LM_REAL_EPSILON+#undef __SUBCNST+#undef LM_CNST+#endif /* LM_SNGL_PREC */++#ifdef LM_DBL_PREC+/* double precision definitions */+#define LM_REAL double+#define LM_PREFIX d++#define LM_REAL_EPSILON DBL_EPSILON+#define LM_CNST(x) (x)++#include "misc_core.c" // read in core code++#undef LM_REAL+#undef LM_PREFIX+#undef LM_REAL_EPSILON+#undef LM_CNST+#endif /* LM_DBL_PREC */
+ levmar-2.4/misc.h view
@@ -0,0 +1,106 @@+/////////////////////////////////////////////////////////////////////////////////+// +// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++#ifndef _MISC_H_+#define _MISC_H_++/* common suffix for LAPACK subroutines. Define empty in case of no prefix. */+#define LM_LAPACK_SUFFIX _+//#define LM_LAPACK_SUFFIX // define empty++/* common prefix for BLAS subroutines. Leave undefined in case of no prefix.+ * You might also need to modify LM_BLAS_PREFIX below+ */+/* f2c'd BLAS */+//#define LM_BLAS_PREFIX f2c_+/* C BLAS */+//#define LM_BLAS_PREFIX cblas_++/* common suffix for BLAS subroutines */+//#define LM_BLAS_SUFFIX // define empty if a f2c_ or cblas_ prefix was defined for LM_BLAS_PREFIX above+#define LM_BLAS_SUFFIX _ // use this in case of no BLAS prefix+++#define LCAT_(a, b) #a b+#define LCAT(a, b) LCAT_(a, b) // force substitution+#define RCAT_(a, b) a #b+#define RCAT(a, b) RCAT_(a, b) // force substitution++#define LM_MK_LAPACK_NAME(s) LM_ADD_PREFIX(LM_CAT_(s, LM_LAPACK_SUFFIX))+++#define __BLOCKSZ__ 32 /* block size for cache-friendly matrix-matrix multiply. It should be+ * such that __BLOCKSZ__^2*sizeof(LM_REAL) is smaller than the CPU (L1)+ * data cache size. Notice that a value of 32 when LM_REAL=double assumes+ * an 8Kb L1 data cache (32*32*8=8K). This is a concervative choice since+ * newer Pentium 4s have a L1 data cache of size 16K, capable of holding+ * up to 45x45 double blocks.+ */+#define __BLOCKSZ__SQ (__BLOCKSZ__)*(__BLOCKSZ__)++/* add a prefix in front of a token */+#define LM_CAT__(a, b) a ## b+#define LM_CAT_(a, b) LM_CAT__(a, b) // force substitution+#define LM_ADD_PREFIX(s) LM_CAT_(LM_PREFIX, s)++#ifdef __cplusplus+extern "C" {+#endif++/* blocking-based matrix multiply */+extern void slevmar_trans_mat_mat_mult(float *a, float *b, int n, int m);+extern void dlevmar_trans_mat_mat_mult(double *a, double *b, int n, int m);++/* forward finite differences */+extern void slevmar_fdif_forw_jac_approx(void (*func)(float *p, float *hx, int m, int n, void *adata),+ float *p, float *hx, float *hxx, float delta,+ float *jac, int m, int n, void *adata);+extern void dlevmar_fdif_forw_jac_approx(void (*func)(double *p, double *hx, int m, int n, void *adata),+ double *p, double *hx, double *hxx, double delta,+ double *jac, int m, int n, void *adata);++/* central finite differences */+extern void slevmar_fdif_cent_jac_approx(void (*func)(float *p, float *hx, int m, int n, void *adata),+ float *p, float *hxm, float *hxp, float delta,+ float *jac, int m, int n, void *adata);+extern void dlevmar_fdif_cent_jac_approx(void (*func)(double *p, double *hx, int m, int n, void *adata),+ double *p, double *hxm, double *hxp, double delta,+ double *jac, int m, int n, void *adata);++/* e=x-y and ||e|| */+extern float slevmar_L2nrmxmy(float *e, float *x, float *y, int n);+extern double dlevmar_L2nrmxmy(double *e, double *x, double *y, int n);++/* covariance of LS fit */+extern int slevmar_covar(float *JtJ, float *C, float sumsq, int m, int n);+extern int dlevmar_covar(double *JtJ, double *C, double sumsq, int m, int n);++/* box constraints consistency check */+extern int slevmar_box_check(float *lb, float *ub, int m);+extern int dlevmar_box_check(double *lb, double *ub, int m);++/* Cholesky */+extern int slevmar_chol(float *C, float *W, int m);+extern int dlevmar_chol(double *C, double *W, int m);++#ifdef __cplusplus+}+#endif++#endif /* _MISC_H_ */
+ levmar-2.4/misc_core.c view
@@ -0,0 +1,813 @@+/////////////////////////////////////////////////////////////////////////////////+//+// Levenberg - Marquardt non-linear minimization algorithm+// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)+// Institute of Computer Science, Foundation for Research & Technology - Hellas+// Heraklion, Crete, Greece.+//+// This program is free software; you can redistribute it and/or modify+// it under the terms of the GNU General Public License as published by+// the Free Software Foundation; either version 2 of the License, or+// (at your option) any later version.+//+// This program is distributed in the hope that it will be useful,+// but WITHOUT ANY WARRANTY; without even the implied warranty of+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the+// GNU General Public License for more details.+//+/////////////////////////////////////////////////////////////////////////////////++#ifndef LM_REAL // not included by misc.c+#error This file should not be compiled directly!+#endif+++/* precision-specific definitions */+#define LEVMAR_CHKJAC LM_ADD_PREFIX(levmar_chkjac)+#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)+#define LEVMAR_FDIF_CENT_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_cent_jac_approx)+#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)+#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)+#define LEVMAR_STDDEV LM_ADD_PREFIX(levmar_stddev)+#define LEVMAR_CORCOEF LM_ADD_PREFIX(levmar_corcoef)+#define LEVMAR_R2 LM_ADD_PREFIX(levmar_R2)+#define LEVMAR_BOX_CHECK LM_ADD_PREFIX(levmar_box_check)+#define LEVMAR_L2NRMXMY LM_ADD_PREFIX(levmar_L2nrmxmy)++#ifdef HAVE_LAPACK+#define LEVMAR_PSEUDOINVERSE LM_ADD_PREFIX(levmar_pseudoinverse)+static int LEVMAR_PSEUDOINVERSE(LM_REAL *A, LM_REAL *B, int m);++/* BLAS matrix multiplication & LAPACK SVD routines */+#ifdef LM_BLAS_PREFIX+#define GEMM LM_CAT_(LM_BLAS_PREFIX, LM_ADD_PREFIX(LM_CAT_(gemm, LM_BLAS_SUFFIX)))+#else+#define GEMM LM_ADD_PREFIX(LM_CAT_(gemm, LM_BLAS_SUFFIX))+#endif+/* C := alpha*op( A )*op( B ) + beta*C */+extern void GEMM(char *transa, char *transb, int *m, int *n, int *k,+ LM_REAL *alpha, LM_REAL *a, int *lda, LM_REAL *b, int *ldb, LM_REAL *beta, LM_REAL *c, int *ldc);++#define GESVD LM_MK_LAPACK_NAME(gesvd)+#define GESDD LM_MK_LAPACK_NAME(gesdd)+extern int GESVD(char *jobu, char *jobvt, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu,+ LM_REAL *vt, int *ldvt, LM_REAL *work, int *lwork, int *info);++/* lapack 3.0 new SVD routine, faster than xgesvd() */+extern int GESDD(char *jobz, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu, LM_REAL *vt, int *ldvt,+ LM_REAL *work, int *lwork, int *iwork, int *info);++/* Cholesky decomposition */+#define POTF2 LM_MK_LAPACK_NAME(potf2)+extern int POTF2(char *uplo, int *n, LM_REAL *a, int *lda, int *info);++#define LEVMAR_CHOLESKY LM_ADD_PREFIX(levmar_chol)++#else+#define LEVMAR_LUINVERSE LM_ADD_PREFIX(levmar_LUinverse_noLapack)++static int LEVMAR_LUINVERSE(LM_REAL *A, LM_REAL *B, int m);+#endif /* HAVE_LAPACK */++/* blocked multiplication of the transpose of the nxm matrix a with itself (i.e. a^T a)+ * using a block size of bsize. The product is returned in b.+ * Since a^T a is symmetric, its computation can be sped up by computing only its+ * upper triangular part and copying it to the lower part.+ *+ * More details on blocking can be found at+ * http://www-2.cs.cmu.edu/afs/cs/academic/class/15213-f02/www/R07/section_a/Recitation07-SectionA.pdf+ */+void LEVMAR_TRANS_MAT_MAT_MULT(LM_REAL *a, LM_REAL *b, int n, int m)+{+#ifdef HAVE_LAPACK /* use BLAS matrix multiply */++LM_REAL alpha=LM_CNST(1.0), beta=LM_CNST(0.0);+ /* Fool BLAS to compute a^T*a avoiding transposing a: a is equivalent to a^T in column major,+ * therefore BLAS computes a*a^T with a and a*a^T in column major, which is equivalent to+ * computing a^T*a in row major!+ */+ GEMM("N", "T", &m, &m, &n, &alpha, a, &m, a, &m, &beta, b, &m);++#else /* no LAPACK, use blocking-based multiply */++register int i, j, k, jj, kk;+register LM_REAL sum, *bim, *akm;+const int bsize=__BLOCKSZ__;++#define __MIN__(x, y) (((x)<=(y))? (x) : (y))+#define __MAX__(x, y) (((x)>=(y))? (x) : (y))++ /* compute upper triangular part using blocking */+ for(jj=0; jj<m; jj+=bsize){+ for(i=0; i<m; ++i){+ bim=b+i*m;+ for(j=__MAX__(jj, i); j<__MIN__(jj+bsize, m); ++j)+ bim[j]=0.0; //b[i*m+j]=0.0;+ }++ for(kk=0; kk<n; kk+=bsize){+ for(i=0; i<m; ++i){+ bim=b+i*m;+ for(j=__MAX__(jj, i); j<__MIN__(jj+bsize, m); ++j){+ sum=0.0;+ for(k=kk; k<__MIN__(kk+bsize, n); ++k){+ akm=a+k*m;+ sum+=akm[i]*akm[j]; //a[k*m+i]*a[k*m+j];+ }+ bim[j]+=sum; //b[i*m+j]+=sum;+ }+ }+ }+ }++ /* copy upper triangular part to the lower one */+ for(i=0; i<m; ++i)+ for(j=0; j<i; ++j)+ b[i*m+j]=b[j*m+i];++#undef __MIN__+#undef __MAX__++#endif /* HAVE_LAPACK */+}++/* forward finite difference approximation to the Jacobian of func */+void LEVMAR_FDIF_FORW_JAC_APPROX(+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),+ /* function to differentiate */+ LM_REAL *p, /* I: current parameter estimate, mx1 */+ LM_REAL *hx, /* I: func evaluated at p, i.e. hx=func(p), nx1 */+ LM_REAL *hxx, /* W/O: work array for evaluating func(p+delta), nx1 */+ LM_REAL delta, /* increment for computing the Jacobian */+ LM_REAL *jac, /* O: array for storing approximated Jacobian, nxm */+ int m,+ int n,+ void *adata)+{+register int i, j;+LM_REAL tmp;+register LM_REAL d;++ for(j=0; j<m; ++j){+ /* determine d=max(1E-04*|p[j]|, delta), see HZ */+ d=LM_CNST(1E-04)*p[j]; // force evaluation+ d=FABS(d);+ if(d<delta)+ d=delta;++ tmp=p[j];+ p[j]+=d;++ (*func)(p, hxx, m, n, adata);++ p[j]=tmp; /* restore */++ d=LM_CNST(1.0)/d; /* invert so that divisions can be carried out faster as multiplications */+ for(i=0; i<n; ++i){+ jac[i*m+j]=(hxx[i]-hx[i])*d;+ }+ }+}++/* central finite difference approximation to the Jacobian of func */+void LEVMAR_FDIF_CENT_JAC_APPROX(+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),+ /* function to differentiate */+ LM_REAL *p, /* I: current parameter estimate, mx1 */+ LM_REAL *hxm, /* W/O: work array for evaluating func(p-delta), nx1 */+ LM_REAL *hxp, /* W/O: work array for evaluating func(p+delta), nx1 */+ LM_REAL delta, /* increment for computing the Jacobian */+ LM_REAL *jac, /* O: array for storing approximated Jacobian, nxm */+ int m,+ int n,+ void *adata)+{+register int i, j;+LM_REAL tmp;+register LM_REAL d;++ for(j=0; j<m; ++j){+ /* determine d=max(1E-04*|p[j]|, delta), see HZ */+ d=LM_CNST(1E-04)*p[j]; // force evaluation+ d=FABS(d);+ if(d<delta)+ d=delta;++ tmp=p[j];+ p[j]-=d;+ (*func)(p, hxm, m, n, adata);++ p[j]=tmp+d;+ (*func)(p, hxp, m, n, adata);+ p[j]=tmp; /* restore */++ d=LM_CNST(0.5)/d; /* invert so that divisions can be carried out faster as multiplications */+ for(i=0; i<n; ++i){+ jac[i*m+j]=(hxp[i]-hxm[i])*d;+ }+ }+}++/*+ * Check the Jacobian of a n-valued nonlinear function in m variables+ * evaluated at a point p, for consistency with the function itself.+ *+ * Based on fortran77 subroutine CHKDER by+ * Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More+ * Argonne National Laboratory. MINPACK project. March 1980.+ *+ *+ * func points to a function from R^m --> R^n: Given a p in R^m it yields hx in R^n+ * jacf points to a function implementing the Jacobian of func, whose correctness+ * is to be tested. Given a p in R^m, jacf computes into the nxm matrix j the+ * Jacobian of func at p. Note that row i of j corresponds to the gradient of+ * the i-th component of func, evaluated at p.+ * p is an input array of length m containing the point of evaluation.+ * m is the number of variables+ * n is the number of functions+ * adata points to possible additional data and is passed uninterpreted+ * to func, jacf.+ * err is an array of length n. On output, err contains measures+ * of correctness of the respective gradients. if there is+ * no severe loss of significance, then if err[i] is 1.0 the+ * i-th gradient is correct, while if err[i] is 0.0 the i-th+ * gradient is incorrect. For values of err between 0.0 and 1.0,+ * the categorization is less certain. In general, a value of+ * err[i] greater than 0.5 indicates that the i-th gradient is+ * probably correct, while a value of err[i] less than 0.5+ * indicates that the i-th gradient is probably incorrect.+ *+ *+ * The function does not perform reliably if cancellation or+ * rounding errors cause a severe loss of significance in the+ * evaluation of a function. therefore, none of the components+ * of p should be unusually small (in particular, zero) or any+ * other value which may cause loss of significance.+ */++void LEVMAR_CHKJAC(+ void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),+ void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),+ LM_REAL *p, int m, int n, void *adata, LM_REAL *err)+{+LM_REAL factor=LM_CNST(100.0);+LM_REAL one=LM_CNST(1.0);+LM_REAL zero=LM_CNST(0.0);+LM_REAL *fvec, *fjac, *pp, *fvecp, *buf;++register int i, j;+LM_REAL eps, epsf, temp, epsmch;+LM_REAL epslog;+int fvec_sz=n, fjac_sz=n*m, pp_sz=m, fvecp_sz=n;++ epsmch=LM_REAL_EPSILON;+ eps=(LM_REAL)sqrt(epsmch);++ buf=(LM_REAL *)malloc((fvec_sz + fjac_sz + pp_sz + fvecp_sz)*sizeof(LM_REAL));+ if(!buf){+ PRINT_ERROR(LCAT(LEVMAR_CHKJAC, "(): memory allocation request failed\n"));+ exit(1);+ }+ fvec=buf;+ fjac=fvec+fvec_sz;+ pp=fjac+fjac_sz;+ fvecp=pp+pp_sz;++ /* compute fvec=func(p) */+ (*func)(p, fvec, m, n, adata);++ /* compute the Jacobian at p */+ (*jacf)(p, fjac, m, n, adata);++ /* compute pp */+ for(j=0; j<m; ++j){+ temp=eps*FABS(p[j]);+ if(temp==zero) temp=eps;+ pp[j]=p[j]+temp;+ }++ /* compute fvecp=func(pp) */+ (*func)(pp, fvecp, m, n, adata);++ epsf=factor*epsmch;+ epslog=(LM_REAL)log10(eps);++ for(i=0; i<n; ++i)+ err[i]=zero;++ for(j=0; j<m; ++j){+ temp=FABS(p[j]);+ if(temp==zero) temp=one;++ for(i=0; i<n; ++i)+ err[i]+=temp*fjac[i*m+j];+ }++ for(i=0; i<n; ++i){+ temp=one;+ if(fvec[i]!=zero && fvecp[i]!=zero && FABS(fvecp[i]-fvec[i])>=epsf*FABS(fvec[i]))+ temp=eps*FABS((fvecp[i]-fvec[i])/eps - err[i])/(FABS(fvec[i])+FABS(fvecp[i]));+ err[i]=one;+ if(temp>epsmch && temp<eps)+ err[i]=((LM_REAL)log10(temp) - epslog)/epslog;+ if(temp>=eps) err[i]=zero;+ }++ free(buf);++ return;+}++#ifdef HAVE_LAPACK+/*+ * This function computes the pseudoinverse of a square matrix A+ * into B using SVD. A and B can coincide+ *+ * The function returns 0 in case of error (e.g. A is singular),+ * the rank of A if successful+ *+ * A, B are mxm+ *+ */+static int LEVMAR_PSEUDOINVERSE(LM_REAL *A, LM_REAL *B, int m)+{+LM_REAL *buf=NULL;+int buf_sz=0;+static LM_REAL eps=LM_CNST(-1.0);++register int i, j;+LM_REAL *a, *u, *s, *vt, *work;+int a_sz, u_sz, s_sz, vt_sz, tot_sz;+LM_REAL thresh, one_over_denom;+int info, rank, worksz, *iwork, iworksz;++ /* calculate required memory size */+ worksz=5*m; // min worksize for GESVD+ //worksz=m*(7*m+4); // min worksize for GESDD+ iworksz=8*m;+ a_sz=m*m;+ u_sz=m*m; s_sz=m; vt_sz=m*m;++ tot_sz=(a_sz + u_sz + s_sz + vt_sz + worksz)*sizeof(LM_REAL) + iworksz*sizeof(int); /* should be arranged in that order for proper doubles alignment */++ buf_sz=tot_sz;+ buf=(LM_REAL *)malloc(buf_sz);+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", LEVMAR_PSEUDOINVERSE) "() failed!\n");+ return 0; /* error */+ }++ a=buf;+ u=a+a_sz;+ s=u+u_sz;+ vt=s+s_sz;+ work=vt+vt_sz;+ iwork=(int *)(work+worksz);++ /* store A (column major!) into a */+ for(i=0; i<m; i++)+ for(j=0; j<m; j++)+ a[i+j*m]=A[i*m+j];++ /* SVD decomposition of A */+ GESVD("A", "A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, &info);+ //GESDD("A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, iwork, &info);++ /* error treatment */+ if(info!=0){+ if(info<0){+ PRINT_ERROR(RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GESVD), "/" GESDD) " in ", LEVMAR_PSEUDOINVERSE) "()\n", -info);+ }+ else{+ PRINT_ERROR(RCAT("LAPACK error: dgesdd (dbdsdc)/dgesvd (dbdsqr) failed to converge in ", LEVMAR_PSEUDOINVERSE) "() [info=%d]\n", info);+ }+ free(buf);+ return 0;+ }++ if(eps<0.0){+ LM_REAL aux;++ /* compute machine epsilon */+ for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))+ ;+ eps*=LM_CNST(2.0);+ }++ /* compute the pseudoinverse in B */+ for(i=0; i<a_sz; i++) B[i]=0.0; /* initialize to zero */+ for(rank=0, thresh=eps*s[0]; rank<m && s[rank]>thresh; rank++){+ one_over_denom=LM_CNST(1.0)/s[rank];++ for(j=0; j<m; j++)+ for(i=0; i<m; i++)+ B[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;+ }++ free(buf);++ return rank;+}+#else // no LAPACK++/*+ * This function computes the inverse of A in B. A and B can coincide+ *+ * The function employs LAPACK-free LU decomposition of A to solve m linear+ * systems A*B_i=I_i, where B_i and I_i are the i-th columns of B and I.+ *+ * A and B are mxm+ *+ * The function returns 0 in case of error, 1 if successful+ *+ */+static int LEVMAR_LUINVERSE(LM_REAL *A, LM_REAL *B, int m)+{+void *buf=NULL;+int buf_sz=0;++register int i, j, k, l;+int *idx, maxi=-1, idx_sz, a_sz, x_sz, work_sz, tot_sz;+LM_REAL *a, *x, *work, max, sum, tmp;++ /* calculate required memory size */+ idx_sz=m;+ a_sz=m*m;+ x_sz=m;+ work_sz=m;+ tot_sz=(a_sz + x_sz + work_sz)*sizeof(LM_REAL) + idx_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */++ buf_sz=tot_sz;+ buf=(void *)malloc(tot_sz);+ if(!buf){+ PRINT_ERROR(RCAT("memory allocation in ", LEVMAR_LUINVERSE) "() failed!\n");+ return 0; /* error */+ }++ a=buf;+ x=a+a_sz;+ work=x+x_sz;+ idx=(int *)(work+work_sz);++ /* avoid destroying A by copying it to a */+ for(i=0; i<a_sz; ++i) a[i]=A[i];++ /* compute the LU decomposition of a row permutation of matrix a; the permutation itself is saved in idx[] */+ for(i=0; i<m; ++i){+ max=0.0;+ for(j=0; j<m; ++j)+ if((tmp=FABS(a[i*m+j]))>max)+ max=tmp;+ if(max==0.0){+ PRINT_ERROR(RCAT("Singular matrix A in ", LEVMAR_LUINVERSE) "()!\n");+ free(buf);++ return 0;+ }+ work[i]=LM_CNST(1.0)/max;+ }++ for(j=0; j<m; ++j){+ for(i=0; i<j; ++i){+ sum=a[i*m+j];+ for(k=0; k<i; ++k)+ sum-=a[i*m+k]*a[k*m+j];+ a[i*m+j]=sum;+ }+ max=0.0;+ for(i=j; i<m; ++i){+ sum=a[i*m+j];+ for(k=0; k<j; ++k)+ sum-=a[i*m+k]*a[k*m+j];+ a[i*m+j]=sum;+ if((tmp=work[i]*FABS(sum))>=max){+ max=tmp;+ maxi=i;+ }+ }+ if(j!=maxi){+ for(k=0; k<m; ++k){+ tmp=a[maxi*m+k];+ a[maxi*m+k]=a[j*m+k];+ a[j*m+k]=tmp;+ }+ work[maxi]=work[j];+ }+ idx[j]=maxi;+ if(a[j*m+j]==0.0)+ a[j*m+j]=LM_REAL_EPSILON;+ if(j!=m-1){+ tmp=LM_CNST(1.0)/(a[j*m+j]);+ for(i=j+1; i<m; ++i)+ a[i*m+j]*=tmp;+ }+ }++ /* The decomposition has now replaced a. Solve the m linear systems using+ * forward and back substitution+ */+ for(l=0; l<m; ++l){+ for(i=0; i<m; ++i) x[i]=0.0;+ x[l]=LM_CNST(1.0);++ for(i=k=0; i<m; ++i){+ j=idx[i];+ sum=x[j];+ x[j]=x[i];+ if(k!=0)+ for(j=k-1; j<i; ++j)+ sum-=a[i*m+j]*x[j];+ else+ if(sum!=0.0)+ k=i+1;+ x[i]=sum;+ }++ for(i=m-1; i>=0; --i){+ sum=x[i];+ for(j=i+1; j<m; ++j)+ sum-=a[i*m+j]*x[j];+ x[i]=sum/a[i*m+i];+ }++ for(i=0; i<m; ++i)+ B[i*m+l]=x[i];+ }++ free(buf);++ return 1;+}+#endif /* HAVE_LAPACK */++/*+ * This function computes in C the covariance matrix corresponding to a least+ * squares fit. JtJ is the approximate Hessian at the solution (i.e. J^T*J, where+ * J is the Jacobian at the solution), sumsq is the sum of squared residuals+ * (i.e. goodnes of fit) at the solution, m is the number of parameters (variables)+ * and n the number of observations. JtJ can coincide with C.+ *+ * if JtJ is of full rank, C is computed as sumsq/(n-m)*(JtJ)^-1+ * otherwise and if LAPACK is available, C=sumsq/(n-r)*(JtJ)^++ * where r is JtJ's rank and ^+ denotes the pseudoinverse+ * The diagonal of C is made up from the estimates of the variances+ * of the estimated regression coefficients.+ * See the documentation of routine E04YCF from the NAG fortran lib+ *+ * The function returns the rank of JtJ if successful, 0 on error+ *+ * A and C are mxm+ *+ */+int LEVMAR_COVAR(LM_REAL *JtJ, LM_REAL *C, LM_REAL sumsq, int m, int n)+{+register int i;+int rnk;+LM_REAL fact;++#ifdef HAVE_LAPACK+ rnk=LEVMAR_PSEUDOINVERSE(JtJ, C, m);+ if(!rnk) return 0;+#else+#ifdef _MSC_VER+#pragma message("LAPACK not available, LU will be used for matrix inversion when computing the covariance; this might be unstable at times")+#else+#warning LAPACK not available, LU will be used for matrix inversion when computing the covariance; this might be unstable at times+#endif // _MSC_VER++ rnk=LEVMAR_LUINVERSE(JtJ, C, m);+ if(!rnk) return 0;++ rnk=m; /* assume full rank */+#endif /* HAVE_LAPACK */++ fact=sumsq/(LM_REAL)(n-rnk);+ for(i=0; i<m*m; ++i)+ C[i]*=fact;++ return rnk;+}++/* standard deviation of the best-fit parameter i.+ * covar is the mxm covariance matrix of the best-fit parameters (see also LEVMAR_COVAR()).+ *+ * The standard deviation is computed as \sigma_{i} = \sqrt{C_{ii}}+ */+LM_REAL LEVMAR_STDDEV(LM_REAL *covar, int m, int i)+{+ return (LM_REAL)sqrt(covar[i*m+i]);+}++/* Pearson's correlation coefficient of the best-fit parameters i and j.+ * covar is the mxm covariance matrix of the best-fit parameters (see also LEVMAR_COVAR()).+ *+ * The coefficient is computed as \rho_{ij} = C_{ij} / sqrt(C_{ii} C_{jj})+ */+LM_REAL LEVMAR_CORCOEF(LM_REAL *covar, int m, int i, int j)+{+ return (LM_REAL)(covar[i*m+j]/sqrt(covar[i*m+i]*covar[j*m+j]));+}++/* coefficient of determination.+ * see http://en.wikipedia.org/wiki/Coefficient_of_determination+ */+LM_REAL LEVMAR_R2(void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),+ LM_REAL *p, LM_REAL *x, int m, int n, void *adata)+{+register int i;+register LM_REAL tmp;+LM_REAL SSerr, // sum of squared errors, i.e. residual sum of squares \sum_i (x_i-hx_i)^2+ SStot, // \sum_i (x_i-xavg)^2+ *hx, xavg;+++ if((hx=(LM_REAL *)malloc(n*sizeof(LM_REAL)))==NULL){+ PRINT_ERROR(RCAT("memory allocation request failed in ", LEVMAR_R2) "()\n");+ exit(1);+ }++ /* hx=f(p) */+ (*func)(p, hx, m, n, adata);++ for(i=0, tmp=0.0; i<n; ++i)+ tmp+=x[i];+ xavg=tmp/(LM_REAL)n;++ for(i=0, SSerr=SStot=0.0; i<n; ++i){+ tmp=x[i]-hx[i];+ SSerr+=tmp*tmp;++ tmp=x[i]-xavg;+ SStot+=tmp*tmp;+ }++ free(hx);++ return LM_CNST(1.0) - SSerr/SStot;+}++/* check box constraints for consistency */+int LEVMAR_BOX_CHECK(LM_REAL *lb, LM_REAL *ub, int m)+{+register int i;++ if(!lb || !ub) return 1;++ for(i=0; i<m; ++i)+ if(lb[i]>ub[i]) return 0;++ return 1;+}++#ifdef HAVE_LAPACK++/* compute the Cholesky decomposition of C in W, s.t. C=W^t W and W is upper triangular */+int LEVMAR_CHOLESKY(LM_REAL *C, LM_REAL *W, int m)+{+register int i, j;+int info;++ /* copy weights array C to W so that LAPACK won't destroy it;+ * C is assumed symmetric, hence no transposition is needed+ */+ for(i=0, j=m*m; i<j; ++i)+ W[i]=C[i];++ /* Cholesky decomposition */+ POTF2("U", (int *)&m, W, (int *)&m, (int *)&info);+ /* error treatment */+ if(info!=0){+ if(info<0){+ PRINT_ERROR("LAPACK error: illegal value for argument %d of dpotf2 in %s\n", -info, LCAT(LEVMAR_CHOLESKY, "()"));+ }+ else{+ PRINT_ERROR("LAPACK error: the leading minor of order %d is not positive definite,\n%s()\n", info,+ RCAT("and the Cholesky factorization could not be completed in ", LEVMAR_CHOLESKY));+ }+ return LM_ERROR_LAPACK_ERROR;+ }++ /* the decomposition is in the upper part of W (in column-major order!).+ * copying it to the lower part and zeroing the upper transposes+ * W in row-major order+ */+ for(i=0; i<m; i++)+ for(j=0; j<i; j++){+ W[i+j*m]=W[j+i*m];+ W[j+i*m]=0.0;+ }++ return 0;+}+#endif /* HAVE_LAPACK */+++/* Compute e=x-y for two n-vectors x and y and return the squared L2 norm of e.+ * e can coincide with either x or y; x can be NULL, in which case it is assumed+ * to be equal to the zero vector.+ * Uses loop unrolling and blocking to reduce bookkeeping overhead & pipeline+ * stalls and increase instruction-level parallelism; see http://www.abarnett.demon.co.uk/tutorial.html+ */++LM_REAL LEVMAR_L2NRMXMY(LM_REAL *e, LM_REAL *x, LM_REAL *y, int n)+{+const int blocksize=8, bpwr=3; /* 8=2^3 */+register int i;+int j1, j2, j3, j4, j5, j6, j7;+int blockn;+register LM_REAL sum0=0.0, sum1=0.0, sum2=0.0, sum3=0.0;++ /* n may not be divisible by blocksize,+ * go as near as we can first, then tidy up.+ */+ blockn = (n>>bpwr)<<bpwr; /* (n / blocksize) * blocksize; */++ /* unroll the loop in blocks of `blocksize'; looping downwards gains some more speed */+ if(x){+ for(i=blockn-1; i>0; i-=blocksize){+ e[i ]=x[i ]-y[i ]; sum0+=e[i ]*e[i ];+ j1=i-1; e[j1]=x[j1]-y[j1]; sum1+=e[j1]*e[j1];+ j2=i-2; e[j2]=x[j2]-y[j2]; sum2+=e[j2]*e[j2];+ j3=i-3; e[j3]=x[j3]-y[j3]; sum3+=e[j3]*e[j3];+ j4=i-4; e[j4]=x[j4]-y[j4]; sum0+=e[j4]*e[j4];+ j5=i-5; e[j5]=x[j5]-y[j5]; sum1+=e[j5]*e[j5];+ j6=i-6; e[j6]=x[j6]-y[j6]; sum2+=e[j6]*e[j6];+ j7=i-7; e[j7]=x[j7]-y[j7]; sum3+=e[j7]*e[j7];+ }++ /*+ * There may be some left to do.+ * This could be done as a simple for() loop,+ * but a switch is faster (and more interesting)+ */++ i=blockn;+ if(i<n){+ /* Jump into the case at the place that will allow+ * us to finish off the appropriate number of items.+ */++ switch(n - i){+ case 7 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;+ case 6 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;+ case 5 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;+ case 4 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;+ case 3 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;+ case 2 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;+ case 1 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;+ }+ }+ }+ else{ /* x==0 */+ for(i=blockn-1; i>0; i-=blocksize){+ e[i ]=-y[i ]; sum0+=e[i ]*e[i ];+ j1=i-1; e[j1]=-y[j1]; sum1+=e[j1]*e[j1];+ j2=i-2; e[j2]=-y[j2]; sum2+=e[j2]*e[j2];+ j3=i-3; e[j3]=-y[j3]; sum3+=e[j3]*e[j3];+ j4=i-4; e[j4]=-y[j4]; sum0+=e[j4]*e[j4];+ j5=i-5; e[j5]=-y[j5]; sum1+=e[j5]*e[j5];+ j6=i-6; e[j6]=-y[j6]; sum2+=e[j6]*e[j6];+ j7=i-7; e[j7]=-y[j7]; sum3+=e[j7]*e[j7];+ }++ /*+ * There may be some left to do.+ * This could be done as a simple for() loop,+ * but a switch is faster (and more interesting)+ */++ i=blockn;+ if(i<n){+ /* Jump into the case at the place that will allow+ * us to finish off the appropriate number of items.+ */++ switch(n - i){+ case 7 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;+ case 6 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;+ case 5 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;+ case 4 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;+ case 3 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;+ case 2 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;+ case 1 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;+ }+ }+ }++ return sum0+sum1+sum2+sum3;+}++/* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */+#undef LEVMAR_PSEUDOINVERSE+#undef LEVMAR_LUINVERSE+#undef LEVMAR_BOX_CHECK+#undef LEVMAR_CHOLESKY+#undef LEVMAR_COVAR+#undef LEVMAR_STDDEV+#undef LEVMAR_CORCOEF+#undef LEVMAR_R2+#undef LEVMAR_CHKJAC+#undef LEVMAR_FDIF_FORW_JAC_APPROX+#undef LEVMAR_FDIF_CENT_JAC_APPROX+#undef LEVMAR_TRANS_MAT_MAT_MULT+#undef LEVMAR_L2NRMXMY