diff --git a/Bindings/LevMar.hsc b/Bindings/LevMar.hsc new file mode 100644 --- /dev/null +++ b/Bindings/LevMar.hsc @@ -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: +-- +-------------------------------------------------------------------------------- + +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 + + +-- | 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 --------------------------------------------------------------------- diff --git a/Bindings/LevMar/CurryFriendly.hs b/Bindings/LevMar/CurryFriendly.hs new file mode 100644 --- /dev/null +++ b/Bindings/LevMar/CurryFriendly.hs @@ -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 --------------------------------------------------------------------- diff --git a/LICENSE b/LICENSE new file mode 100644 --- /dev/null +++ b/LICENSE @@ -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. diff --git a/Setup.hs b/Setup.hs new file mode 100644 --- /dev/null +++ b/Setup.hs @@ -0,0 +1,3 @@ +import Distribution.Simple + +main = defaultMain diff --git a/bindings-levmar.cabal b/bindings-levmar.cabal new file mode 100644 --- /dev/null +++ b/bindings-levmar.cabal @@ -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: + . + 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 diff --git a/levmar-2.4/Axb.c b/levmar-2.4/Axb.c new file mode 100644 --- /dev/null +++ b/levmar-2.4/Axb.c @@ -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 +#include +#include + +#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 +#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 */ diff --git a/levmar-2.4/Axb_core.c b/levmar-2.4/Axb_core.c new file mode 100644 --- /dev/null +++ b/levmar-2.4/Axb_core.c @@ -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; ibuf_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; ibuf_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; ibuf_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; ibuf_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; i0.0; eps*=LM_CNST(0.5)) + ; + eps*=LM_CNST(2.0); + } + + /* compute the pseudoinverse in a */ + for(i=0; ithresh; rank++){ + one_over_denom=LM_CNST(1.0)/s[rank]; + + for(j=0; jbuf_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; imax) + 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=max){ + max=tmp; + maxi=i; + } + } + if(j!=maxi){ + for(k=0; k=0; --i){ + sum=x[i]; + for(j=i+1; j + Copyright (C) + + 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. + + , 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. diff --git a/levmar-2.4/Makefile b/levmar-2.4/Makefile new file mode 100644 --- /dev/null +++ b/levmar-2.4/Makefile @@ -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. + diff --git a/levmar-2.4/Makefile.icc b/levmar-2.4/Makefile.icc new file mode 100644 --- /dev/null +++ b/levmar-2.4/Makefile.icc @@ -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. + diff --git a/levmar-2.4/Makefile.vc b/levmar-2.4/Makefile.vc new file mode 100644 --- /dev/null +++ b/levmar-2.4/Makefile.vc @@ -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 + diff --git a/levmar-2.4/README.txt b/levmar-2.4/README.txt new file mode 100644 --- /dev/null +++ b/levmar-2.4/README.txt @@ -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 diff --git a/levmar-2.4/compiler.h b/levmar-2.4/compiler.h new file mode 100644 --- /dev/null +++ b/levmar-2.4/compiler.h @@ -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_ */ diff --git a/levmar-2.4/expfit.c b/levmar-2.4/expfit.c new file mode 100644 --- /dev/null +++ b/levmar-2.4/expfit.c @@ -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 +#include +#include + +#include + +#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 +#define GETPID _getpid +#elif defined(__GNUC__) // GCC +#include +#include +#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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/levmar-2.4/lm.c b/levmar-2.4/lm.c new file mode 100644 --- /dev/null +++ b/levmar-2.4/lm.c @@ -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 +#include +#include +#include + +#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 */ diff --git a/levmar-2.4/lm.h b/levmar-2.4/lm.h new file mode 100644 --- /dev/null +++ b/levmar-2.4/lm.h @@ -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_ */ diff --git a/levmar-2.4/lm_core.c b/levmar-2.4/lm_core.c new file mode 100644 --- /dev/null +++ b/levmar-2.4/lm_core.c @@ -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(n0; ) + 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; jtmp) 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=(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; i0.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=itmax) stop=3; + + for(i=0; i=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(n16) || 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; jtmp) tmp=diag_jacTjac[i]; /* find max diagonal element */ + mu=tau*tmp; + } + + /* determine increment using adaptive damping */ + + /* augment normal equations */ + for(i=0; i=(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; i0){ /* update jac */ + for(i=0; i0.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=itmax) stop=3; + + for(i=0; i +#include +#include +#include + +#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 */ diff --git a/levmar-2.4/lmbc_core.c b/levmar-2.4/lmbc_core.c new file mode 100644 --- /dev/null +++ b/levmar-2.4/lmbc_core.c @@ -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=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 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; iub[i]) p[i]=ub[i]; + } + } + else + if(!ub){ /* lower bounds only */ + for(i=0; i 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(n0; ) + 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; j0; + * if p[i]==lb[i] g[i]<0; otherwise g[i]=0 + */ + for(i=j=numactive=0, p_L2=jacTe_inf=0.0; i0.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; itmp) 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=(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; i0.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=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; itming; t*=beta){ + for(i=0; i=itmax) stop=3; + + for(i=0; ifunc))(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 diff --git a/levmar-2.4/lmblec.c b/levmar-2.4/lmblec.c new file mode 100644 --- /dev/null +++ b/levmar-2.4/lmblec.c @@ -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 +#include +#include +#include + +#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 */ diff --git a/levmar-2.4/lmblec_core.c b/levmar-2.4/lmblec_core.c new file mode 100644 --- /dev/null +++ b/levmar-2.4/lmblec_core.c @@ -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; ilb; + 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 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 +#include +#include +#include + +#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=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=-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=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=-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=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 +#include +#include + +#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 */ + diff --git a/levmar-2.4/lmlec_core.c b/levmar-2.4/lmlec_core.c new file mode 100644 --- /dev/null +++ b/levmar-2.4/lmlec_core.c @@ -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; i0){ + 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; itmp || a[i*(tm+1)]<-tmp) ++rank; /* loop across R's diagonal elements */ + else break; /* diagonal is arranged in absolute decreasing order */ + + if(rank0){ + 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; i0){ + 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; incnstr; + c=data->c; + Z=data->Z; + p=data->p; + /* p=c + Z*pp */ + for(i=0; ifunc))(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; ijacf))(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; iLM_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; iLM_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 -L 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 diff --git a/levmar-2.4/matlab/bt3.m b/levmar-2.4/matlab/bt3.m new file mode 100644 --- /dev/null +++ b/levmar-2.4/matlab/bt3.m @@ -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 diff --git a/levmar-2.4/matlab/expfit.m b/levmar-2.4/matlab/expfit.m new file mode 100644 --- /dev/null +++ b/levmar-2.4/matlab/expfit.m @@ -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 diff --git a/levmar-2.4/matlab/hs01.m b/levmar-2.4/matlab/hs01.m new file mode 100644 --- /dev/null +++ b/levmar-2.4/matlab/hs01.m @@ -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); diff --git a/levmar-2.4/matlab/jacbt3.m b/levmar-2.4/matlab/jacbt3.m new file mode 100644 --- /dev/null +++ b/levmar-2.4/matlab/jacbt3.m @@ -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 + diff --git a/levmar-2.4/matlab/jacexpfit.m b/levmar-2.4/matlab/jacexpfit.m new file mode 100644 --- /dev/null +++ b/levmar-2.4/matlab/jacexpfit.m @@ -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 diff --git a/levmar-2.4/matlab/jachs01.m b/levmar-2.4/matlab/jachs01.m new file mode 100644 --- /dev/null +++ b/levmar-2.4/matlab/jachs01.m @@ -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]; diff --git a/levmar-2.4/matlab/jacmeyer.m b/levmar-2.4/matlab/jacmeyer.m new file mode 100644 --- /dev/null +++ b/levmar-2.4/matlab/jacmeyer.m @@ -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 diff --git a/levmar-2.4/matlab/jacmodhs52.m b/levmar-2.4/matlab/jacmodhs52.m new file mode 100644 --- /dev/null +++ b/levmar-2.4/matlab/jacmodhs52.m @@ -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]; diff --git a/levmar-2.4/matlab/levmar.c b/levmar-2.4/matlab/levmar.c new file mode 100644 --- /dev/null +++ b/levmar-2.4/matlab/levmar.c @@ -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 +#include +#include +#include +#include +#include + +#include + +#include + +/** +#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; inrhs, dat->rhs, dat->fname); + + /* copy back results & cleanup */ + mx=mxGetPr(lhs[0]); + for(i=0; irhs[0]); + for(i=0; inrhs, 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; irhs[0]); + for(i=0; inrhs, 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; iLM_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=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; i3) /* 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; i2){ + plhs[2]=mxCreateDoubleMatrix(1, LM_INFO_SZ, mxREAL); + pdbl=mxGetPr(plhs[2]); + for(i=0; i3){ + plhs[3]=mxCreateDoubleMatrix(m, m, mxREAL); + pdbl=mxGetPr(plhs[3]); + for(i=0; i +#include +#include +#include + +#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 */ diff --git a/levmar-2.4/misc.h b/levmar-2.4/misc.h new file mode 100644 --- /dev/null +++ b/levmar-2.4/misc.h @@ -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_ */ diff --git a/levmar-2.4/misc_core.c b/levmar-2.4/misc_core.c new file mode 100644 --- /dev/null +++ b/levmar-2.4/misc_core.c @@ -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 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=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]=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; i0.0; eps*=LM_CNST(0.5)) + ; + eps*=LM_CNST(2.0); + } + + /* compute the pseudoinverse in B */ + for(i=0; ithresh; rank++){ + one_over_denom=LM_CNST(1.0)/s[rank]; + + for(j=0; jmax) + 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=max){ + max=tmp; + maxi=i; + } + } + if(j!=maxi){ + for(k=0; k=0; --i){ + sum=x[i]; + for(j=i+1; jub[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>bpwr)<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(i0; 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