lucaspalomodevelop/meshlab
1
0
Fork 0
mirror of https://github.com/lucaspalomodevelop/meshlab.git synced 2026-03-13 00:07:24 +00:00
Code Issues Packages Projects Releases Wiki Activity

remove levmar source and download it using cmake, update to levmar 2.6

Browse Source
This commit is contained in:
alemuntoni 2022-11-01 18:47:39 +01:00
parent 45768bbae7
commit 1c135c59ff
48 changed files with 28 additions and 8112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
src/external/levmar-2.3/Axb.c vendored
View File

@ -1,74 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// Solution of linear systems involved in the Levenberg - Marquardt
// minimization algorithm
// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)
// Institute of Computer Science, Foundation for Research & Technology - Hellas
// Heraklion, Crete, Greece.
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
/////////////////////////////////////////////////////////////////////////////////
/********************************************************************************
* LAPACK-based implementations for various linear system solvers. The same core
* code is used with appropriate #defines to derive single and double precision
* solver versions, see also Axb_core.c
********************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "lm.h"
#include "misc.h"
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
#endif
#ifdef LM_DBL_PREC
/* double precision definitions */
#define LM_REAL double
#define LM_PREFIX d
#define LM_CNST(x) (x)
#ifndef HAVE_LAPACK
#include <float.h>
#define LM_REAL_EPSILON DBL_EPSILON
#endif
#include "Axb_core.c"
#undef LM_REAL
#undef LM_PREFIX
#undef LM_CNST
#undef LM_REAL_EPSILON
#endif /* LM_DBL_PREC */
#ifdef LM_SNGL_PREC
/* single precision (float) definitions */
#define LM_REAL float
#define LM_PREFIX s
#define __SUBCNST(x) x##F
#define LM_CNST(x) __SUBCNST(x) // force substitution
#ifndef HAVE_LAPACK
#define LM_REAL_EPSILON FLT_EPSILON
#endif
#include "Axb_core.c"
#undef LM_REAL
#undef LM_PREFIX
#undef __SUBCNST
#undef LM_CNST
#undef LM_REAL_EPSILON
#endif /* LM_SNGL_PREC */

1001
src/external/levmar-2.3/Axb_core.c vendored
View File

File diff suppressed because it is too large Load Diff

52
src/external/levmar-2.3/CMakeLists.txt vendored
View File

@ -1,52 +0,0 @@
# levmar CMake file; see http://www.cmake.org and
# http://www.insightsoftwareconsortium.org/wiki/index.php/CMake_Tutorial
PROJECT(LEVMAR)
#CMAKE_MINIMUM_REQUIRED(VERSION 1.4)
# compiler flags
ADD_DEFINITIONS(-DLINSOLVERS_RETAIN_MEMORY) # do not free memory between linear solvers calls
#REMOVE_DEFINITIONS(-DLINSOLVERS_RETAIN_MEMORY)
# f2c is sometimes equivalent to libF77 & libI77; in that case, set HAVE_F2C to 0
SET(HAVE_F2C 1 CACHE BOOL "Do we have f2c or F77/I77?" )
# the directory where the lapack/blas/f2c libraries reside
SET(LAPACKBLAS_DIR /usr/lib CACHE PATH "Path to lapack/blas libraries")
# actual names for the lapack/blas/f2c libraries
SET(LAPACK_LIB lapack CACHE STRING "The name of the lapack library")
SET(BLAS_LIB blas CACHE STRING "The name of the blas library")
IF(HAVE_F2C)
SET(F2C_LIB f2c CACHE STRING "The name of the f2c library")
ELSE(HAVE_F2C)
SET(F77_LIB libF77 CACHE STRING "The name of the F77 library")
SET(I77_LIB libI77 CACHE STRING "The name of the I77 library")
ENDIF(HAVE_F2C)
########################## NO CHANGES BEYOND THIS POINT ##########################
#INCLUDE_DIRECTORIES(/usr/include)
LINK_DIRECTORIES(${LAPACKBLAS_DIR})
# levmar library source files
ADD_LIBRARY(levmar STATIC
lm.c Axb.c misc.c lmlec.c lmbc.c lmblec.c
lm.h misc.h compiler.h
)
# demo program
ADD_EXECUTABLE(lmdemo lmdemo.c lm.h)
# libraries the demo depends on
IF(HAVE_F2C)
TARGET_LINK_LIBRARIES(lmdemo levmar ${LAPACK_LIB} ${BLAS_LIB} ${F2C_LIB})
ELSE(HAVE_F2C)
TARGET_LINK_LIBRARIES(lmdemo levmar ${LAPACK_LIB} ${BLAS_LIB} ${F77_LIB} ${I77_LIB})
ENDIF(HAVE_F2C)
# make sure that the library is built before the demo
ADD_DEPENDENCIES(lmdemo levmar)
#SUBDIRS(matlab)
#ADD_TEST(levmar_tst lmdemo)

340
src/external/levmar-2.3/LICENSE vendored
View File

@ -1,340 +0,0 @@
GNU GENERAL PUBLIC LICENSE
Version 2, June 1991
Copyright (C) 1989, 1991 Free Software Foundation, Inc.
59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The licenses for most software are designed to take away your
freedom to share and change it. By contrast, the GNU General Public
License is intended to guarantee your freedom to share and change free
software--to make sure the software is free for all its users. This
General Public License applies to most of the Free Software
Foundation's software and to any other program whose authors commit to
using it. (Some other Free Software Foundation software is covered by
the GNU Library General Public License instead.) You can apply it to
your programs, too.
When we speak of free software, we are referring to freedom, not
price. Our General Public Licenses are designed to make sure that you
have the freedom to distribute copies of free software (and charge for
this service if you wish), that you receive source code or can get it
if you want it, that you can change the software or use pieces of it
in new free programs; and that you know you can do these things.
To protect your rights, we need to make restrictions that forbid
anyone to deny you these rights or to ask you to surrender the rights.
These restrictions translate to certain responsibilities for you if you
distribute copies of the software, or if you modify it.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must give the recipients all the rights that
you have. You must make sure that they, too, receive or can get the
source code. And you must show them these terms so they know their
rights.
We protect your rights with two steps: (1) copyright the software, and
(2) offer you this license which gives you legal permission to copy,
distribute and/or modify the software.
Also, for each author's protection and ours, we want to make certain
that everyone understands that there is no warranty for this free
software. If the software is modified by someone else and passed on, we
want its recipients to know that what they have is not the original, so
that any problems introduced by others will not reflect on the original
authors' reputations.
Finally, any free program is threatened constantly by software
patents. We wish to avoid the danger that redistributors of a free
program will individually obtain patent licenses, in effect making the
program proprietary. To prevent this, we have made it clear that any
patent must be licensed for everyone's free use or not licensed at all.
The precise terms and conditions for copying, distribution and
modification follow.
GNU GENERAL PUBLIC LICENSE
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
0. This License applies to any program or other work which contains
a notice placed by the copyright holder saying it may be distributed
under the terms of this General Public License. The "Program", below,
refers to any such program or work, and a "work based on the Program"
means either the Program or any derivative work under copyright law:
that is to say, a work containing the Program or a portion of it,
either verbatim or with modifications and/or translated into another
language. (Hereinafter, translation is included without limitation in
the term "modification".) Each licensee is addressed as "you".
Activities other than copying, distribution and modification are not
covered by this License; they are outside its scope. The act of
running the Program is not restricted, and the output from the Program
is covered only if its contents constitute a work based on the
Program (independent of having been made by running the Program).
Whether that is true depends on what the Program does.
1. You may copy and distribute verbatim copies of the Program's
source code as you receive it, in any medium, provided that you
conspicuously and appropriately publish on each copy an appropriate
copyright notice and disclaimer of warranty; keep intact all the
notices that refer to this License and to the absence of any warranty;
and give any other recipients of the Program a copy of this License
along with the Program.
You may charge a fee for the physical act of transferring a copy, and
you may at your option offer warranty protection in exchange for a fee.
2. You may modify your copy or copies of the Program or any portion
of it, thus forming a work based on the Program, and copy and
distribute such modifications or work under the terms of Section 1
above, provided that you also meet all of these conditions:
a) You must cause the modified files to carry prominent notices
stating that you changed the files and the date of any change.
b) You must cause any work that you distribute or publish, that in
whole or in part contains or is derived from the Program or any
part thereof, to be licensed as a whole at no charge to all third
parties under the terms of this License.
c) If the modified program normally reads commands interactively
when run, you must cause it, when started running for such
interactive use in the most ordinary way, to print or display an
announcement including an appropriate copyright notice and a
notice that there is no warranty (or else, saying that you provide
a warranty) and that users may redistribute the program under
these conditions, and telling the user how to view a copy of this
License. (Exception: if the Program itself is interactive but
does not normally print such an announcement, your work based on
the Program is not required to print an announcement.)
These requirements apply to the modified work as a whole. If
identifiable sections of that work are not derived from the Program,
and can be reasonably considered independent and separate works in
themselves, then this License, and its terms, do not apply to those
sections when you distribute them as separate works. But when you
distribute the same sections as part of a whole which is a work based
on the Program, the distribution of the whole must be on the terms of
this License, whose permissions for other licensees extend to the
entire whole, and thus to each and every part regardless of who wrote it.
Thus, it is not the intent of this section to claim rights or contest
your rights to work written entirely by you; rather, the intent is to
exercise the right to control the distribution of derivative or
collective works based on the Program.
In addition, mere aggregation of another work not based on the Program
with the Program (or with a work based on the Program) on a volume of
a storage or distribution medium does not bring the other work under
the scope of this License.
3. You may copy and distribute the Program (or a work based on it,
under Section 2) in object code or executable form under the terms of
Sections 1 and 2 above provided that you also do one of the following:
a) Accompany it with the complete corresponding machine-readable
source code, which must be distributed under the terms of Sections
1 and 2 above on a medium customarily used for software interchange; or,
b) Accompany it with a written offer, valid for at least three
years, to give any third party, for a charge no more than your
cost of physically performing source distribution, a complete
machine-readable copy of the corresponding source code, to be
distributed under the terms of Sections 1 and 2 above on a medium
customarily used for software interchange; or,
c) Accompany it with the information you received as to the offer
to distribute corresponding source code. (This alternative is
allowed only for noncommercial distribution and only if you
received the program in object code or executable form with such
an offer, in accord with Subsection b above.)
The source code for a work means the preferred form of the work for
making modifications to it. For an executable work, complete source
code means all the source code for all modules it contains, plus any
associated interface definition files, plus the scripts used to
control compilation and installation of the executable. However, as a
special exception, the source code distributed need not include
anything that is normally distributed (in either source or binary
form) with the major components (compiler, kernel, and so on) of the
operating system on which the executable runs, unless that component
itself accompanies the executable.
If distribution of executable or object code is made by offering
access to copy from a designated place, then offering equivalent
access to copy the source code from the same place counts as
distribution of the source code, even though third parties are not
compelled to copy the source along with the object code.
4. You may not copy, modify, sublicense, or distribute the Program
except as expressly provided under this License. Any attempt
otherwise to copy, modify, sublicense or distribute the Program is
void, and will automatically terminate your rights under this License.
However, parties who have received copies, or rights, from you under
this License will not have their licenses terminated so long as such
parties remain in full compliance.
5. You are not required to accept this License, since you have not
signed it. However, nothing else grants you permission to modify or
distribute the Program or its derivative works. These actions are
prohibited by law if you do not accept this License. Therefore, by
modifying or distributing the Program (or any work based on the
Program), you indicate your acceptance of this License to do so, and
all its terms and conditions for copying, distributing or modifying
the Program or works based on it.
6. Each time you redistribute the Program (or any work based on the
Program), the recipient automatically receives a license from the
original licensor to copy, distribute or modify the Program subject to
these terms and conditions. You may not impose any further
restrictions on the recipients' exercise of the rights granted herein.
You are not responsible for enforcing compliance by third parties to
this License.
7. If, as a consequence of a court judgment or allegation of patent
infringement or for any other reason (not limited to patent issues),
conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot
distribute so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you
may not distribute the Program at all. For example, if a patent
license would not permit royalty-free redistribution of the Program by
all those who receive copies directly or indirectly through you, then
the only way you could satisfy both it and this License would be to
refrain entirely from distribution of the Program.
If any portion of this section is held invalid or unenforceable under
any particular circumstance, the balance of the section is intended to
apply and the section as a whole is intended to apply in other
circumstances.
It is not the purpose of this section to induce you to infringe any
patents or other property right claims or to contest validity of any
such claims; this section has the sole purpose of protecting the
integrity of the free software distribution system, which is
implemented by public license practices. Many people have made
generous contributions to the wide range of software distributed
through that system in reliance on consistent application of that
system; it is up to the author/donor to decide if he or she is willing
to distribute software through any other system and a licensee cannot
impose that choice.
This section is intended to make thoroughly clear what is believed to
be a consequence of the rest of this License.
8. If the distribution and/or use of the Program is restricted in
certain countries either by patents or by copyrighted interfaces, the
original copyright holder who places the Program under this License
may add an explicit geographical distribution limitation excluding
those countries, so that distribution is permitted only in or among
countries not thus excluded. In such case, this License incorporates
the limitation as if written in the body of this License.
9. The Free Software Foundation may publish revised and/or new versions
of the General Public License from time to time. Such new versions will
be similar in spirit to the present version, but may differ in detail to
address new problems or concerns.
Each version is given a distinguishing version number. If the Program
specifies a version number of this License which applies to it and "any
later version", you have the option of following the terms and conditions
either of that version or of any later version published by the Free
Software Foundation. If the Program does not specify a version number of
this License, you may choose any version ever published by the Free Software
Foundation.
10. If you wish to incorporate parts of the Program into other free
programs whose distribution conditions are different, write to the author
to ask for permission. For software which is copyrighted by the Free
Software Foundation, write to the Free Software Foundation; we sometimes
make exceptions for this. Our decision will be guided by the two goals
of preserving the free status of all derivatives of our free software and
of promoting the sharing and reuse of software generally.
NO WARRANTY
11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
REPAIR OR CORRECTION.
12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
POSSIBILITY OF SUCH DAMAGES.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Programs
If you develop a new program, and you want it to be of the greatest
possible use to the public, the best way to achieve this is to make it
free software which everyone can redistribute and change under these terms.
To do so, attach the following notices to the program. It is safest
to attach them to the start of each source file to most effectively
convey the exclusion of warranty; and each file should have at least
the "copyright" line and a pointer to where the full notice is found.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
Also add information on how to contact you by electronic and paper mail.
If the program is interactive, make it output a short notice like this
when it starts in an interactive mode:
Gnomovision version 69, Copyright (C) year name of author
Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it
under certain conditions; type `show c' for details.
The hypothetical commands `show w' and `show c' should show the appropriate
parts of the General Public License. Of course, the commands you use may
be called something other than `show w' and `show c'; they could even be
mouse-clicks or menu items--whatever suits your program.
You should also get your employer (if you work as a programmer) or your
school, if any, to sign a "copyright disclaimer" for the program, if
necessary. Here is a sample; alter the names:
Yoyodyne, Inc., hereby disclaims all copyright interest in the program
`Gnomovision' (which makes passes at compilers) written by James Hacker.
<signature of Ty Coon>, 1 April 1989
Ty Coon, President of Vice
This General Public License does not permit incorporating your program into
proprietary programs. If your program is a subroutine library, you may
consider it more useful to permit linking proprietary applications with the
library. If this is what you want to do, use the GNU Library General
Public License instead of this License.

58
src/external/levmar-2.3/Makefile.icc vendored
View File

@ -1,58 +0,0 @@
#
# 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.

58
src/external/levmar-2.3/Makefile.vc vendored
View File

@ -1,58 +0,0 @@
#
# 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

70
src/external/levmar-2.3/README.txt vendored
View File

@ -1,70 +0,0 @@
**************************************************************
LEVMAR
version 2.3
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.
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
- 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

41
src/external/levmar-2.3/compiler.h vendored
View File

@ -1,41 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// 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_ */

122
src/external/levmar-2.3/expfit.c vendored
View File

@ -1,122 +0,0 @@
////////////////////////////////////////////////////////////////////////////////////
// Example program that shows how to use levmar in order to fit the three-
// parameter exponential model x_i = p[0]*exp(-p[1]*i) + p[2] to a set of
// data measurements; example is based on a similar one from GSL.
//
// Copyright (C) 2008 Manolis Lourakis (lourakis at ics forth gr)
// Institute of Computer Science, Foundation for Research & Technology - Hellas
// Heraklion, Crete, Greece.
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
////////////////////////////////////////////////////////////////////////////////////
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <lm.h>
#ifndef LM_DBL_PREC
#error Example program assumes that levmar has been compiled with double precision, see LM_DBL_PREC!
#endif
/* the following macros concern the initialization of a random number generator for adding noise */
#undef REPEATABLE_RANDOM
#define DBL_RAND_MAX (double)(RAND_MAX)
#ifdef _MSC_VER // MSVC
#include <process.h>
#define GETPID _getpid
#elif defined(__GNUC__) // GCC
#include <sys/types.h>
#include <unistd.h>
#define GETPID getpid
#else
#warning Do not know the name of the function returning the process id for your OS/compiler combination
#define GETPID 0
#endif /* _MSC_VER */
#ifdef REPEATABLE_RANDOM
#define INIT_RANDOM(seed) srandom(seed)
#else
#define INIT_RANDOM(seed) srandom((int)GETPID()) // seed unused
#endif
/* Gaussian noise with mean m and variance s, uses the Box-Muller transformation */
double gNoise(double m, double s)
{
double r1, r2, val;
r1=((double)random())/DBL_RAND_MAX;
r2=((double)random())/DBL_RAND_MAX;
val=sqrt(-2.0*log(r1))*cos(2.0*M_PI*r2);
val=s*val+m;
return val;
}
/* model to be fitted to measurements: x_i = p[0]*exp(-p[1]*i) + p[2], i=0...n-1 */
void expfunc(double *p, double *x, int m, int n, void *data)
{
register int i;
for(i=0; i<n; ++i){
x[i]=p[0]*exp(-p[1]*i) + p[2];
}
}
/* Jacobian of expfunc() */
void jacexpfunc(double *p, double *jac, int m, int n, void *data)
{
register int i, j;
/* fill Jacobian row by row */
for(i=j=0; i<n; ++i){
jac[j++]=exp(-p[1]*i);
jac[j++]=-p[0]*i*exp(-p[1]*i);
jac[j++]=1.0;
}
}
int main()
{
const int n=40, m=3; // 40 measurements, 3 parameters
double p[m], x[n], opts[LM_OPTS_SZ], info[LM_INFO_SZ];
register int i;
int ret;
/* generate some measurement using the exponential model with
* parameters (5.0, 0.1, 1.0), corrupted with zero-mean
* Gaussian noise of s=0.1
*/
INIT_RANDOM(0);
for(i=0; i<n; ++i)
x[i]=(5.0*exp(-0.1*i) + 1.0) + gNoise(0.0, 0.1);
/* initial parameters estimate: (1.0, 0.0, 0.0) */
p[0]=1.0; p[1]=0.0; p[2]=0.0;
/* optimization control parameters; passing to levmar NULL instead of opts reverts to defaults */
opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20;
opts[4]=LM_DIFF_DELTA; // relevant only if the finite difference Jacobian version is used
/* invoke the optimization function */
ret=dlevmar_der(expfunc, jacexpfunc, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
//ret=dlevmar_dif(expfunc, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // without Jacobian
printf("Levenberg-Marquardt returned in %g iter, reason %g, sumsq %g [%g]\n", info[5], info[6], info[1], info[0]);
printf("Best fit parameters: %.7g %.7g %.7g\n", p[0], p[1], p[2]);
exit(0);
}

83
src/external/levmar-2.3/lm.c vendored
View File

@ -1,83 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// Levenberg - Marquardt non-linear minimization algorithm
// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)
// Institute of Computer Science, Foundation for Research & Technology - Hellas
// Heraklion, Crete, Greece.
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
/////////////////////////////////////////////////////////////////////////////////
/********************************************************************************
* Levenberg-Marquardt nonlinear minimization. The same core code is used with
* appropriate #defines to derive single and double precision versions, see
* also lm_core.c
********************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include "lm.h"
#include "compiler.h"
#include "misc.h"
#define EPSILON 1E-12
#define ONE_THIRD 0.3333333334 /* 1.0/3.0 */
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
#endif
#ifdef LM_SNGL_PREC
/* single precision (float) definitions */
#define LM_REAL float
#define LM_PREFIX s
#define LM_REAL_MAX FLT_MAX
#define LM_REAL_MIN -FLT_MAX
#define LM_REAL_EPSILON FLT_EPSILON
#define __SUBCNST(x) x##F
#define LM_CNST(x) __SUBCNST(x) // force substitution
#include "lm_core.c" // read in core code
#undef LM_REAL
#undef LM_PREFIX
#undef LM_REAL_MAX
#undef LM_REAL_EPSILON
#undef LM_REAL_MIN
#undef __SUBCNST
#undef LM_CNST
#endif /* LM_SNGL_PREC */
#ifdef LM_DBL_PREC
/* double precision definitions */
#define LM_REAL double
#define LM_PREFIX d
#define LM_REAL_MAX DBL_MAX
#define LM_REAL_MIN -DBL_MAX
#define LM_REAL_EPSILON DBL_EPSILON
#define LM_CNST(x) (x)
#include "lm_core.c" // read in core code
#undef LM_REAL
#undef LM_PREFIX
#undef LM_REAL_MAX
#undef LM_REAL_EPSILON
#undef LM_REAL_MIN
#undef LM_CNST
#endif /* LM_DBL_PREC */

259
src/external/levmar-2.3/lm.h vendored
View File

@ -1,259 +0,0 @@
/*
////////////////////////////////////////////////////////////////////////////////////
//
// 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 */
/****************** End of configuration options, no changes necessary beyond this point ******************/
#ifdef __cplusplus
extern "C" {
#endif
#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 9
#define LM_ERROR -1
#define LM_INIT_MU 1E-03
#define LM_STOP_THRESH 1E-17
#define LM_DIFF_DELTA 1E-06
#define LM_VERSION "2.3 (May 2008)"
#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 & 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);
#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);
#endif /* LM_SNGL_PREC */
#ifdef __cplusplus
}
#endif
#endif /* _LM_H_ */

810
src/external/levmar-2.3/lm_core.c vendored
View File

@ -1,810 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// 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 successfull, LM_ERROR if failed
*
* 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
*/
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;
const int nm=n*m;
int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;
mu=jacTe_inf=0.0; /* -Wall */
if(n<m){
fprintf(stderr, LCAT(LEVMAR_DER, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);
return LM_ERROR;
}
if(!jacf){
fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_DER)
RCAT("().\nIf no such function is available, use ", LEVMAR_DIF) RCAT("() rather than ", LEVMAR_DER) "()\n");
return LM_ERROR;
}
if(opts){
tau=opts[0];
eps1=opts[1];
eps2=opts[2];
eps2_sq=opts[2]*opts[2];
eps3=opts[3];
}
else{ // use default values
tau=LM_CNST(LM_INIT_MU);
eps1=LM_CNST(LM_STOP_THRESH);
eps2=LM_CNST(LM_STOP_THRESH);
eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);
eps3=LM_CNST(LM_STOP_THRESH);
}
if(!work){
worksz=LM_DER_WORKSZ(m, n); //2*n+4*m + n*m + m*m;
work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */
if(!work){
fprintf(stderr, LCAT(LEVMAR_DER, "(): memory allocation request failed\n"));
exit(1);
}
freework=1;
}
/* set up work arrays */
e=work;
hx=e + n;
jacTe=hx + n;
jac=jacTe + m;
jacTjac=jac + nm;
Dp=jacTjac + m*m;
diag_jacTjac=Dp + m;
pDp=diag_jacTjac + m;
/* compute e=x - f(p) and its L2 norm */
(*func)(p, hx, m, n, adata); nfev=1;
/* ### e=x-hx, p_eL2=||e|| */
#if 1
p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);
#else
for(i=0, p_eL2=0.0; i<n; ++i){
e[i]=tmp=x[i]-hx[i];
p_eL2+=tmp*tmp;
}
#endif
init_p_eL2=p_eL2;
if(!LM_FINITE(p_eL2)) stop=7;
for(k=0; k<itmax && !stop; ++k){
/* Note that p and e have been updated at a previous iteration */
if(p_eL2<=eps3){ /* error is small */
stop=6;
break;
}
/* Compute the Jacobian J at p, J^T J, J^T e, ||J^T e||_inf and ||p||^2.
* Since J^T J is symmetric, its computation can be speeded up by computing
* only its upper triangular part and copying it to the lower part
*/
(*jacf)(p, jac, m, n, adata); ++njev;
/* J^T J, J^T e */
if(nm<__BLOCKSZ__SQ){ // this is a small problem
/* This is the straightforward way to compute J^T J, J^T e. 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 J is too large to
* fit in the L1 cache. For such problems, a cache-efficient blocking scheme
* is preferable.
*
* Thanks to John Nitao of Lawrence Livermore Lab for pointing out this
* performance problem.
*
* On the other hand, the straightforward algorithm is faster on small
* problems since in this case it avoids the overheads of blocking.
*/
for(i=0; i<m; ++i){
for(j=i; j<m; ++j){
int lm;
for(l=0, tmp=0.0; l<n; ++l){
lm=l*m;
tmp+=jac[lm+i]*jac[lm+j];
}
/* store tmp in the corresponding upper and lower part elements */
jacTjac[i*m+j]=jacTjac[j*m+i]=tmp;
}
/* J^T e */
for(l=0, tmp=0.0; l<n; ++l)
tmp+=jac[l*m+i]*e[l];
jacTe[i]=tmp;
}
}
else{ // this is a large problem
/* Cache efficient computation of J^T J based on blocking
*/
LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);
/* cache efficient computation of J^T e */
for(i=0; i<m; ++i)
jacTe[i]=0.0;
for(i=0; i<n; ++i){
register LM_REAL *jacrow;
for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)
jacTe[l]+=jacrow[l]*tmp;
}
}
/* Compute ||J^T e||_inf and ||p||^2 */
for(i=0, p_L2=jacTe_inf=0.0; i<m; ++i){
if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;
diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */
p_L2+=p[i]*p[i];
}
//p_L2=sqrt(p_L2);
#if 0
if(!(k%100)){
printf("Current estimate: ");
for(i=0; i<m; ++i)
printf("%.9g ", p[i]);
printf("-- errors %.9g %0.9g\n", jacTe_inf, p_eL2);
}
#endif
/* check for convergence */
if((jacTe_inf <= eps1)){
Dp_L2=0.0; /* no increment for p in this case */
stop=1;
break;
}
/* compute initial damping factor */
if(k==0){
for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */
mu=tau*tmp;
}
/* determine increment using adaptive damping */
while(1){
/* augment normal equations */
for(i=0; i<m; ++i)
jacTjac[i*m+i]+=mu;
/* solve augmented equations */
#ifdef HAVE_LAPACK
/* 5 alternatives are available: LU, Cholesky, 2 variants of QR decomposition and SVD.
* Cholesky is the fastest but might be inaccurate; QR is slower but more accurate;
* SVD is the slowest but most accurate; LU offers a tradeoff between accuracy and speed
*/
issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_LU;
//issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_CHOL;
//issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_QR;
//issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); linsolver=AX_EQ_B_QRLS;
//issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_SVD;
#else
/* use the LU included with levmar */
issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_LU;
#endif /* HAVE_LAPACK */
if(issolved){
/* compute p's new estimate and ||Dp||^2 */
for(i=0, Dp_L2=0.0; i<m; ++i){
pDp[i]=p[i] + (tmp=Dp[i]);
Dp_L2+=tmp*tmp;
}
//Dp_L2=sqrt(Dp_L2);
if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
//if(Dp_L2<=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */
stop=2;
break;
}
if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */
//if(Dp_L2>=(p_L2+eps2)/LM_CNST(EPSILON)){ /* almost singular */
stop=4;
break;
}
(*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + Dp */
/* compute ||e(pDp)||_2 */
/* ### hx=x-hx, pDp_eL2=||hx|| */
#if 1
pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);
#else
for(i=0, pDp_eL2=0.0; i<n; ++i){
hx[i]=tmp=x[i]-hx[i];
pDp_eL2+=tmp*tmp;
}
#endif
if(!LM_FINITE(pDp_eL2)){ /* sum of squares is not finite, most probably due to a user error.
* This check makes sure that the inner loop does not run indefinitely.
* Thanks to Steve Danauskas for reporting such cases
*/
stop=7;
break;
}
for(i=0, dL=0.0; i<m; ++i)
dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);
dF=p_eL2-pDp_eL2;
if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */
tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));
tmp=LM_CNST(1.0)-tmp*tmp*tmp;
mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );
nu=2;
for(i=0 ; i<m; ++i) /* update p's estimate */
p[i]=pDp[i];
for(i=0; i<n; ++i) /* update e and ||e||_2 */
e[i]=hx[i];
p_eL2=pDp_eL2;
break;
}
}
/* if this point is reached, either the linear system could not be solved or
* the error did not reduce; in any case, the increment must be rejected
*/
mu*=nu;
nu2=nu<<1; // 2*nu;
if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */
stop=5;
break;
}
nu=nu2;
for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
jacTjac[i*m+i]=diag_jacTjac[i];
} /* inner loop */
}
if(k>=itmax) stop=3;
for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
jacTjac[i*m+i]=diag_jacTjac[i];
if(info){
info[0]=init_p_eL2;
info[1]=p_eL2;
info[2]=jacTe_inf;
info[3]=Dp_L2;
for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];
info[4]=mu/tmp;
info[5]=(LM_REAL)k;
info[6]=(LM_REAL)stop;
info[7]=(LM_REAL)nfev;
info[8]=(LM_REAL)njev;
}
/* covariance matrix */
if(covar){
LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);
}
if(freework) free(work);
#ifdef LINSOLVERS_RETAIN_MEMORY
if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);
#endif
return (stop!=4 && stop!=7)? k : LM_ERROR;
}
/* Secant version of the LEVMAR_DER() function above: the Jacobian is approximated with
* the aid of finite differences (forward or central, see the comment for the opts argument)
*/
int LEVMAR_DIF(
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */
LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */
LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */
int m, /* I: parameter vector dimension (i.e. #unknowns) */
int n, /* I: measurement vector dimension */
int itmax, /* I: maximum number of iterations */
LM_REAL opts[5], /* I: opts[0-4] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
* scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
* the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
* If \delta<0, the Jacobian is approximated with central differences which are more accurate
* (but slower!) compared to the forward differences employed by default.
*/
LM_REAL info[LM_INFO_SZ],
/* O: information regarding the minimization. Set to NULL if don't care
* info[0]= ||e||_2 at initial p.
* info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
* info[5]= # iterations,
* info[6]=reason for terminating: 1 - stopped by small gradient J^T e
* 2 - stopped by small Dp
* 3 - stopped by itmax
* 4 - singular matrix. Restart from current p with increased mu
* 5 - no further error reduction is possible. Restart with increased mu
* 6 - stopped by small ||e||_2
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
* info[7]= # function evaluations
* info[8]= # Jacobian evaluations
*/
LM_REAL *work, /* working memory at least LM_DIF_WORKSZ() reals large, allocated if NULL */
LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
void *adata) /* pointer to possibly additional data, passed uninterpreted to func.
* Set to NULL if not needed
*/
{
register int i, j, k, l;
int worksz, freework=0, issolved;
/* temp work arrays */
LM_REAL *e, /* nx1 */
*hx, /* \hat{x}_i, nx1 */
*jacTe, /* J^T e_i mx1 */
*jac, /* nxm */
*jacTjac, /* mxm */
*Dp, /* mx1 */
*diag_jacTjac, /* diagonal of J^T J, mx1 */
*pDp, /* p + Dp, mx1 */
*wrk, /* nx1 */
*wrk2; /* nx1, used only for holding a temporary e vector and when differentiating with central differences */
int using_ffdif=1;
register LM_REAL mu, /* damping constant */
tmp; /* mainly used in matrix & vector multiplications */
LM_REAL p_eL2, jacTe_inf, pDp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+Dp)||_2 */
LM_REAL p_L2, Dp_L2=LM_REAL_MAX, dF, dL;
LM_REAL tau, eps1, eps2, eps2_sq, eps3, delta;
LM_REAL init_p_eL2;
int nu, nu2, stop=0, nfev, njap=0, K=(m>=10)? m: 10, updjac, updp=1, newjac;
const int nm=n*m;
int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;
mu=jacTe_inf=p_L2=0.0; /* -Wall */
updjac=newjac=0; /* -Wall */
if(n<m){
fprintf(stderr, LCAT(LEVMAR_DIF, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);
return LM_ERROR;
}
if(opts){
tau=opts[0];
eps1=opts[1];
eps2=opts[2];
eps2_sq=opts[2]*opts[2];
eps3=opts[3];
delta=opts[4];
if(delta<0.0){
delta=-delta; /* make positive */
using_ffdif=0; /* use central differencing */
}
}
else{ // use default values
tau=LM_CNST(LM_INIT_MU);
eps1=LM_CNST(LM_STOP_THRESH);
eps2=LM_CNST(LM_STOP_THRESH);
eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);
eps3=LM_CNST(LM_STOP_THRESH);
delta=LM_CNST(LM_DIFF_DELTA);
}
if(!work){
worksz=LM_DIF_WORKSZ(m, n); //4*n+4*m + n*m + m*m;
work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */
if(!work){
fprintf(stderr, LCAT(LEVMAR_DIF, "(): memory allocation request failed\n"));
exit(1);
}
freework=1;
}
/* set up work arrays */
e=work;
hx=e + n;
jacTe=hx + n;
jac=jacTe + m;
jacTjac=jac + nm;
Dp=jacTjac + m*m;
diag_jacTjac=Dp + m;
pDp=diag_jacTjac + m;
wrk=pDp + m;
wrk2=wrk + n;
/* compute e=x - f(p) and its L2 norm */
(*func)(p, hx, m, n, adata); nfev=1;
/* ### e=x-hx, p_eL2=||e|| */
#if 1
p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);
#else
for(i=0, p_eL2=0.0; i<n; ++i){
e[i]=tmp=x[i]-hx[i];
p_eL2+=tmp*tmp;
}
#endif
init_p_eL2=p_eL2;
if(!LM_FINITE(p_eL2)) stop=7;
nu=20; /* force computation of J */
for(k=0; k<itmax && !stop; ++k){
/* Note that p and e have been updated at a previous iteration */
if(p_eL2<=eps3){ /* error is small */
stop=6;
break;
}
/* Compute the Jacobian J at p, J^T J, J^T e, ||J^T e||_inf and ||p||^2.
* The symmetry of J^T J is again exploited for speed
*/
if((updp && nu>16) || updjac==K){ /* compute difference approximation to J */
if(using_ffdif){ /* use forward differences */
LEVMAR_FDIF_FORW_JAC_APPROX(func, p, hx, wrk, delta, jac, m, n, adata);
++njap; nfev+=m;
}
else{ /* use central differences */
LEVMAR_FDIF_CENT_JAC_APPROX(func, p, wrk, wrk2, delta, jac, m, n, adata);
++njap; nfev+=2*m;
}
nu=2; updjac=0; updp=0; newjac=1;
}
if(newjac){ /* Jacobian has changed, recompute J^T J, J^t e, etc */
newjac=0;
/* J^T J, J^T e */
if(nm<=__BLOCKSZ__SQ){ // this is a small problem
/* This is the straightforward way to compute J^T J, J^T e. 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 J is too large to
* fit in the L1 cache. For such problems, a cache-efficient blocking scheme
* is preferable.
*
* Thanks to John Nitao of Lawrence Livermore Lab for pointing out this
* performance problem.
*
* On the other hand, the straightforward algorithm is faster on small
* problems since in this case it avoids the overheads of blocking.
*/
for(i=0; i<m; ++i){
for(j=i; j<m; ++j){
int lm;
for(l=0, tmp=0.0; l<n; ++l){
lm=l*m;
tmp+=jac[lm+i]*jac[lm+j];
}
jacTjac[i*m+j]=jacTjac[j*m+i]=tmp;
}
/* J^T e */
for(l=0, tmp=0.0; l<n; ++l)
tmp+=jac[l*m+i]*e[l];
jacTe[i]=tmp;
}
}
else{ // this is a large problem
/* Cache efficient computation of J^T J based on blocking
*/
LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);
/* cache efficient computation of J^T e */
for(i=0; i<m; ++i)
jacTe[i]=0.0;
for(i=0; i<n; ++i){
register LM_REAL *jacrow;
for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)
jacTe[l]+=jacrow[l]*tmp;
}
}
/* Compute ||J^T e||_inf and ||p||^2 */
for(i=0, p_L2=jacTe_inf=0.0; i<m; ++i){
if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;
diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */
p_L2+=p[i]*p[i];
}
//p_L2=sqrt(p_L2);
}
#if 0
if(!(k%100)){
printf("Current estimate: ");
for(i=0; i<m; ++i)
printf("%.9g ", p[i]);
printf("-- errors %.9g %0.9g\n", jacTe_inf, p_eL2);
}
#endif
/* check for convergence */
if((jacTe_inf <= eps1)){
Dp_L2=0.0; /* no increment for p in this case */
stop=1;
break;
}
/* compute initial damping factor */
if(k==0){
for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */
mu=tau*tmp;
}
/* determine increment using adaptive damping */
/* augment normal equations */
for(i=0; i<m; ++i)
jacTjac[i*m+i]+=mu;
/* solve augmented equations */
#ifdef HAVE_LAPACK
/* 5 alternatives are available: LU, Cholesky, 2 variants of QR decomposition and SVD.
* Cholesky is the fastest but might be inaccurate; QR is slower but more accurate;
* SVD is the slowest but most accurate; LU offers a tradeoff between accuracy and speed
*/
issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_LU;
//issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_CHOL;
//issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_QR;
//issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); linsolver=AX_EQ_B_QRLS;
//issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_SVD;
#else
/* use the LU included with levmar */
issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_LU;
#endif /* HAVE_LAPACK */
if(issolved){
/* compute p's new estimate and ||Dp||^2 */
for(i=0, Dp_L2=0.0; i<m; ++i){
pDp[i]=p[i] + (tmp=Dp[i]);
Dp_L2+=tmp*tmp;
}
//Dp_L2=sqrt(Dp_L2);
if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
//if(Dp_L2<=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */
stop=2;
break;
}
if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */
//if(Dp_L2>=(p_L2+eps2)/LM_CNST(EPSILON)){ /* almost singular */
stop=4;
break;
}
(*func)(pDp, wrk, m, n, adata); ++nfev; /* evaluate function at p + Dp */
/* compute ||e(pDp)||_2 */
/* ### wrk2=x-wrk, pDp_eL2=||wrk2|| */
#if 1
pDp_eL2=LEVMAR_L2NRMXMY(wrk2, x, wrk, n);
#else
for(i=0, pDp_eL2=0.0; i<n; ++i){
wrk2[i]=tmp=x[i]-wrk[i];
pDp_eL2+=tmp*tmp;
}
#endif
if(!LM_FINITE(pDp_eL2)){ /* sum of squares is not finite, most probably due to a user error.
* This check makes sure that the loop terminates early in the case
* of invalid input. Thanks to Steve Danauskas for suggesting it
*/
stop=7;
break;
}
dF=p_eL2-pDp_eL2;
if(updp || dF>0){ /* update jac */
for(i=0; i<n; ++i){
for(l=0, tmp=0.0; l<m; ++l)
tmp+=jac[i*m+l]*Dp[l]; /* (J * Dp)[i] */
tmp=(wrk[i] - hx[i] - tmp)/Dp_L2; /* (f(p+dp)[i] - f(p)[i] - (J * Dp)[i])/(dp^T*dp) */
for(j=0; j<m; ++j)
jac[i*m+j]+=tmp*Dp[j];
}
++updjac;
newjac=1;
}
for(i=0, dL=0.0; i<m; ++i)
dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);
if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */
tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));
tmp=LM_CNST(1.0)-tmp*tmp*tmp;
mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );
nu=2;
for(i=0 ; i<m; ++i) /* update p's estimate */
p[i]=pDp[i];
for(i=0; i<n; ++i){ /* update e, hx and ||e||_2 */
e[i]=wrk2[i]; //x[i]-wrk[i];
hx[i]=wrk[i];
}
p_eL2=pDp_eL2;
updp=1;
continue;
}
}
/* if this point is reached, either the linear system could not be solved or
* the error did not reduce; in any case, the increment must be rejected
*/
mu*=nu;
nu2=nu<<1; // 2*nu;
if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */
stop=5;
break;
}
nu=nu2;
for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
jacTjac[i*m+i]=diag_jacTjac[i];
}
if(k>=itmax) stop=3;
for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
jacTjac[i*m+i]=diag_jacTjac[i];
if(info){
info[0]=init_p_eL2;
info[1]=p_eL2;
info[2]=jacTe_inf;
info[3]=Dp_L2;
for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];
info[4]=mu/tmp;
info[5]=(LM_REAL)k;
info[6]=(LM_REAL)stop;
info[7]=(LM_REAL)nfev;
info[8]=(LM_REAL)njap;
}
/* covariance matrix */
if(covar){
LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);
}
if(freework) free(work);
#ifdef LINSOLVERS_RETAIN_MEMORY
if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);
#endif
return (stop!=4 && stop!=7)? k : LM_ERROR;
}
/* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */
#undef LEVMAR_DER
#undef LEVMAR_DIF
#undef LEVMAR_FDIF_FORW_JAC_APPROX
#undef LEVMAR_FDIF_CENT_JAC_APPROX
#undef LEVMAR_COVAR
#undef LEVMAR_TRANS_MAT_MAT_MULT
#undef LEVMAR_L2NRMXMY
#undef AX_EQ_B_LU
#undef AX_EQ_B_CHOL
#undef AX_EQ_B_QR
#undef AX_EQ_B_QRLS
#undef AX_EQ_B_SVD

85
src/external/levmar-2.3/lmbc.c vendored
View File

@ -1,85 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// Levenberg - Marquardt non-linear minimization algorithm
// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)
// Institute of Computer Science, Foundation for Research & Technology - Hellas
// Heraklion, Crete, Greece.
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
/////////////////////////////////////////////////////////////////////////////////
/********************************************************************************
* Box-constrained Levenberg-Marquardt nonlinear minimization. The same core code
* is used with appropriate #defines to derive single and double precision versions,
* see also lmbc_core.c
********************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include "lm.h"
#include "compiler.h"
#include "misc.h"
#define EPSILON 1E-12
#define ONE_THIRD 0.3333333334 /* 1.0/3.0 */
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
#endif
#ifdef LM_SNGL_PREC
/* single precision (float) definitions */
#define LM_REAL float
#define LM_PREFIX s
#define LM_REAL_MAX FLT_MAX
#define LM_REAL_MIN -FLT_MAX
#define LM_REAL_EPSILON FLT_EPSILON
#define __SUBCNST(x) x##F
#define LM_CNST(x) __SUBCNST(x) // force substitution
#include "lmbc_core.c" // read in core code
#undef LM_REAL
#undef LM_PREFIX
#undef LM_REAL_MAX
#undef LM_REAL_MIN
#undef LM_REAL_EPSILON
#undef __SUBCNST
#undef LM_CNST
#endif /* LM_SNGL_PREC */
#ifdef LM_DBL_PREC
/* double precision definitions */
#define LM_REAL double
#define LM_PREFIX d
#define LM_REAL_MAX DBL_MAX
#define LM_REAL_MIN -DBL_MAX
#define LM_REAL_EPSILON DBL_EPSILON
#define LM_CNST(x) (x)
#include "lmbc_core.c" // read in core code
#undef LM_REAL
#undef LM_PREFIX
#undef LM_REAL_MAX
#undef LM_REAL_MIN
#undef LM_REAL_EPSILON
#undef LM_CNST
#endif /* LM_DBL_PREC */

919
src/external/levmar-2.3/lmbc_core.c vendored
View File

@ -1,919 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// 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) //CHECKME
#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
#define LEVMAR_FDIF_CENT_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_cent_jac_approx)
#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
#define LEVMAR_L2NRMXMY LM_ADD_PREFIX(levmar_L2nrmxmy)
#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
#define LMBC_DIF_DATA LM_ADD_PREFIX(lmbc_dif_data)
#define LMBC_DIF_FUNC LM_ADD_PREFIX(lmbc_dif_func)
#define LMBC_DIF_JACF LM_ADD_PREFIX(lmbc_dif_jacf)
#ifdef HAVE_LAPACK
#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU)
#define AX_EQ_B_CHOL LM_ADD_PREFIX(Ax_eq_b_Chol)
#define AX_EQ_B_QR LM_ADD_PREFIX(Ax_eq_b_QR)
#define AX_EQ_B_QRLS LM_ADD_PREFIX(Ax_eq_b_QRLS)
#define AX_EQ_B_SVD LM_ADD_PREFIX(Ax_eq_b_SVD)
#else
#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU_noLapack)
#endif /* HAVE_LAPACK */
/* find the median of 3 numbers */
#define __MEDIAN3(a, b, c) ( ((a) >= (b))?\
( ((c) >= (a))? (a) : ( ((c) <= (b))? (b) : (c) ) ) : \
( ((c) >= (b))? (b) : ( ((c) <= (a))? (a) : (c) ) ) )
#define _POW_ LM_CNST(2.1)
#define __LSITMAX 150 // max #iterations for line search
struct FUNC_STATE{
int n, *nfev;
LM_REAL *hx, *x;
void *adata;
};
static void
LNSRCH(int m, LM_REAL *x, LM_REAL f, LM_REAL *g, LM_REAL *p, LM_REAL alpha, LM_REAL *xpls,
LM_REAL *ffpls, void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), struct FUNC_STATE state,
int *mxtake, int *iretcd, LM_REAL stepmx, LM_REAL steptl, LM_REAL *sx)
{
/* Find a next newton iterate by backtracking line search.
* Specifically, finds a \lambda such that for a fixed alpha<0.5 (usually 1e-4),
* f(x + \lambda*p) <= f(x) + alpha * \lambda * g^T*p
*
* Translated (with minor changes) from Schnabel, Koontz & Weiss uncmin.f, v1.3
* PARAMETERS :
* m --> dimension of problem (i.e. number of variables)
* x(m) --> old iterate: x[k-1]
* f --> function value at old iterate, f(x)
* g(m) --> gradient at old iterate, g(x), or approximate
* p(m) --> non-zero newton step
* alpha --> fixed constant < 0.5 for line search (see above)
* xpls(m) <-- new iterate x[k]
* ffpls <-- function value at new iterate, f(xpls)
* func --> name of subroutine to evaluate function
* state <--> information other than x and m that func requires.
* state is not modified in xlnsrch (but can be modified by func).
* iretcd <-- return code
* mxtake <-- boolean flag indicating step of maximum length used
* stepmx --> maximum allowable step size
* steptl --> relative step size at which successive iterates
* considered close enough to terminate algorithm
* sx(m) --> diagonal scaling matrix for x, can be NULL
* internal variables
* sln newton length
* rln relative length of newton step
*/
register int i, j;
int firstback = 1;
LM_REAL disc;
LM_REAL a3, b;
LM_REAL t1, t2, t3, lambda, tlmbda, rmnlmb;
LM_REAL scl, rln, sln, slp;
LM_REAL tmp1, tmp2;
LM_REAL fpls, pfpls = 0., plmbda = 0.; /* -Wall */
f*=LM_CNST(0.5);
*mxtake = 0;
*iretcd = 2;
tmp1 = 0.;
if(!sx) /* no scaling */
for (i = 0; i < m; ++i)
tmp1 += p[i] * p[i];
else
for (i = 0; i < m; ++i)
tmp1 += sx[i] * sx[i] * p[i] * p[i];
sln = (LM_REAL)sqrt(tmp1);
if (sln > stepmx) {
/* newton step longer than maximum allowed */
scl = stepmx / sln;
for(i=0; i<m; ++i) /* p * scl */
p[i]*=scl;
sln = stepmx;
}
for(i=0, slp=0.; i<m; ++i) /* g^T * p */
slp+=g[i]*p[i];
rln = 0.;
if(!sx) /* no scaling */
for (i = 0; i < m; ++i) {
tmp1 = (FABS(x[i])>=LM_CNST(1.))? FABS(x[i]) : LM_CNST(1.);
tmp2 = FABS(p[i])/tmp1;
if(rln < tmp2) rln = tmp2;
}
else
for (i = 0; i < m; ++i) {
tmp1 = (FABS(x[i])>=LM_CNST(1.)/sx[i])? FABS(x[i]) : LM_CNST(1.)/sx[i];
tmp2 = FABS(p[i])/tmp1;
if(rln < tmp2) rln = tmp2;
}
rmnlmb = steptl / rln;
lambda = LM_CNST(1.0);
/* check if new iterate satisfactory. generate new lambda if necessary. */
for(j=__LSITMAX; j>=0; --j) {
for (i = 0; i < m; ++i)
xpls[i] = x[i] + lambda * p[i];
/* evaluate function at new point */
(*func)(xpls, state.hx, m, state.n, state.adata); ++(*(state.nfev));
/* ### state.hx=state.x-state.hx, tmp1=||state.hx|| */
#if 1
tmp1=LEVMAR_L2NRMXMY(state.hx, state.x, state.hx, state.n);
#else
for(i=0, tmp1=0.0; i<state.n; ++i){
state.hx[i]=tmp2=state.x[i]-state.hx[i];
tmp1+=tmp2*tmp2;
}
#endif
fpls=LM_CNST(0.5)*tmp1; *ffpls=tmp1;
if (fpls <= f + slp * alpha * lambda) { /* solution found */
*iretcd = 0;
if (lambda == LM_CNST(1.) && sln > stepmx * LM_CNST(.99)) *mxtake = 1;
return;
}
/* else : solution not (yet) found */
/* First find a point with a finite value */
if (lambda < rmnlmb) {
/* no satisfactory xpls found sufficiently distinct from x */
*iretcd = 1;
return;
}
else { /* calculate new lambda */
/* modifications to cover non-finite values */
if (!LM_FINITE(fpls)) {
lambda *= LM_CNST(0.1);
firstback = 1;
}
else {
if (firstback) { /* first backtrack: quadratic fit */
tlmbda = -lambda * slp / ((fpls - f - slp) * LM_CNST(2.));
firstback = 0;
}
else { /* all subsequent backtracks: cubic fit */
t1 = fpls - f - lambda * slp;
t2 = pfpls - f - plmbda * slp;
t3 = LM_CNST(1.) / (lambda - plmbda);
a3 = LM_CNST(3.) * t3 * (t1 / (lambda * lambda)
- t2 / (plmbda * plmbda));
b = t3 * (t2 * lambda / (plmbda * plmbda)
- t1 * plmbda / (lambda * lambda));
disc = b * b - a3 * slp;
if (disc > b * b)
/* only one positive critical point, must be minimum */
tlmbda = (-b + ((a3 < 0)? -(LM_REAL)sqrt(disc): (LM_REAL)sqrt(disc))) /a3;
else
/* both critical points positive, first is minimum */
tlmbda = (-b + ((a3 < 0)? (LM_REAL)sqrt(disc): -(LM_REAL)sqrt(disc))) /a3;
if (tlmbda > lambda * LM_CNST(.5))
tlmbda = lambda * LM_CNST(.5);
}
plmbda = lambda;
pfpls = fpls;
if (tlmbda < lambda * LM_CNST(.1))
lambda *= LM_CNST(.1);
else
lambda = tlmbda;
}
}
}
/* this point is reached when the iterations limit is exceeded */
*iretcd = 1; /* failed */
return;
} /* LNSRCH */
/* Projections to feasible set \Omega: P_{\Omega}(y) := arg min { ||x - y|| : x \in \Omega}, y \in R^m */
/* project vector p to a box shaped feasible set. p is a mx1 vector.
* Either lb, ub can be NULL. If not NULL, they are mx1 vectors
*/
static void BOXPROJECT(LM_REAL *p, LM_REAL *lb, LM_REAL *ub, int m)
{
register int i;
if(!lb){ /* no lower bounds */
if(!ub) /* no upper bounds */
return;
else{ /* upper bounds only */
for(i=0; i<m; ++i)
if(p[i]>ub[i]) p[i]=ub[i];
}
}
else
if(!ub){ /* lower bounds only */
for(i=0; i<m; ++i)
if(p[i]<lb[i]) p[i]=lb[i];
}
else /* box bounds */
for(i=0; i<m; ++i)
p[i]=__MEDIAN3(lb[i], p[i], ub[i]);
}
/*
* This function seeks the parameter vector p that best describes the measurements
* vector x under box constraints.
* More precisely, given a vector function func : R^m --> R^n with n>=m,
* it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of
* e=x-func(p) is minimized under the constraints lb[i]<=p[i]<=ub[i].
* If no lower bound constraint applies for p[i], use -DBL_MAX/-FLT_MAX for lb[i];
* If no upper bound constraint applies for p[i], use DBL_MAX/FLT_MAX for ub[i].
*
* This function requires an analytic Jacobian. In case the latter is unavailable,
* use LEVMAR_BC_DIF() bellow
*
* Returns the number of iterations (>=0) if successfull, LM_ERROR if failed
*
* 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
*/
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;
const int nm=n*m;
/* variables for constrained LM */
struct FUNC_STATE fstate;
LM_REAL alpha=LM_CNST(1e-4), beta=LM_CNST(0.9), gamma=LM_CNST(0.99995), gamma_sq=gamma*gamma, rho=LM_CNST(1e-8);
LM_REAL t, t0;
LM_REAL steptl=LM_CNST(1e3)*(LM_REAL)sqrt(LM_REAL_EPSILON), jacTeDp;
LM_REAL tmin=LM_CNST(1e-12), tming=LM_CNST(1e-18); /* minimum step length for LS and PG steps */
const LM_REAL tini=LM_CNST(1.0); /* initial step length for LS and PG steps */
int nLMsteps=0, nLSsteps=0, nPGsteps=0, gprevtaken=0;
int numactive;
int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;
mu=jacTe_inf=t=0.0; tmin=tmin; /* -Wall */
if(n<m){
fprintf(stderr, LCAT(LEVMAR_BC_DER, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);
return LM_ERROR;
}
if(!jacf){
fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_BC_DER)
RCAT("().\nIf no such function is available, use ", LEVMAR_BC_DIF) RCAT("() rather than ", LEVMAR_BC_DER) "()\n");
return LM_ERROR;
}
if(!LEVMAR_BOX_CHECK(lb, ub, m)){
fprintf(stderr, LCAT(LEVMAR_BC_DER, "(): at least one lower bound exceeds the upper one\n"));
return LM_ERROR;
}
if(opts){
tau=opts[0];
eps1=opts[1];
eps2=opts[2];
eps2_sq=opts[2]*opts[2];
eps3=opts[3];
}
else{ // use default values
tau=LM_CNST(LM_INIT_MU);
eps1=LM_CNST(LM_STOP_THRESH);
eps2=LM_CNST(LM_STOP_THRESH);
eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);
eps3=LM_CNST(LM_STOP_THRESH);
}
if(!work){
worksz=LM_BC_DER_WORKSZ(m, n); //2*n+4*m + n*m + m*m;
work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */
if(!work){
fprintf(stderr, LCAT(LEVMAR_BC_DER, "(): memory allocation request failed\n"));
exit(1);
}
freework=1;
}
/* set up work arrays */
e=work;
hx=e + n;
jacTe=hx + n;
jac=jacTe + m;
jacTjac=jac + nm;
Dp=jacTjac + m*m;
diag_jacTjac=Dp + m;
pDp=diag_jacTjac + m;
fstate.n=n;
fstate.hx=hx;
fstate.x=x;
fstate.adata=adata;
fstate.nfev=&nfev;
/* see if starting point is within the feasile set */
for(i=0; i<m; ++i)
pDp[i]=p[i];
BOXPROJECT(p, lb, ub, m); /* project to feasible set */
for(i=0; i<m; ++i)
if(pDp[i]!=p[i])
fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_BC_DER) "()! [%g projected to %g]\n",
i, pDp[i], p[i]);
/* compute e=x - f(p) and its L2 norm */
(*func)(p, hx, m, n, adata); nfev=1;
/* ### e=x-hx, p_eL2=||e|| */
#if 1
p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);
#else
for(i=0, p_eL2=0.0; i<n; ++i){
e[i]=tmp=x[i]-hx[i];
p_eL2+=tmp*tmp;
}
#endif
init_p_eL2=p_eL2;
if(!LM_FINITE(p_eL2)) stop=7;
for(k=0; k<itmax && !stop; ++k){
/* Note that p and e have been updated at a previous iteration */
if(p_eL2<=eps3){ /* error is small */
stop=6;
break;
}
/* Compute the Jacobian J at p, J^T J, J^T e, ||J^T e||_inf and ||p||^2.
* Since J^T J is symmetric, its computation can be speeded up by computing
* only its upper triangular part and copying it to the lower part
*/
(*jacf)(p, jac, m, n, adata); ++njev;
/* J^T J, J^T e */
if(nm<__BLOCKSZ__SQ){ // this is a small problem
/* This is the straightforward way to compute J^T J, J^T e. 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 J is too large to
* fit in the L1 cache. For such problems, a cache-efficient blocking scheme
* is preferable.
*
* Thanks to John Nitao of Lawrence Livermore Lab for pointing out this
* performance problem.
*
* On the other hand, the straightforward algorithm is faster on small
* problems since in this case it avoids the overheads of blocking.
*/
for(i=0; i<m; ++i){
for(j=i; j<m; ++j){
int lm;
for(l=0, tmp=0.0; l<n; ++l){
lm=l*m;
tmp+=jac[lm+i]*jac[lm+j];
}
/* store tmp in the corresponding upper and lower part elements */
jacTjac[i*m+j]=jacTjac[j*m+i]=tmp;
}
/* J^T e */
for(l=0, tmp=0.0; l<n; ++l)
tmp+=jac[l*m+i]*e[l];
jacTe[i]=tmp;
}
}
else{ // this is a large problem
/* Cache efficient computation of J^T J based on blocking
*/
LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);
/* cache efficient computation of J^T e */
for(i=0; i<m; ++i)
jacTe[i]=0.0;
for(i=0; i<n; ++i){
register LM_REAL *jacrow;
for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)
jacTe[l]+=jacrow[l]*tmp;
}
}
/* Compute ||J^T e||_inf and ||p||^2. Note that ||J^T e||_inf
* is computed for free (i.e. inactive) variables only.
* At a local minimum, if p[i]==ub[i] then g[i]>0;
* if p[i]==lb[i] g[i]<0; otherwise g[i]=0
*/
for(i=j=numactive=0, p_L2=jacTe_inf=0.0; i<m; ++i){
if(ub && p[i]==ub[i]){ ++numactive; if(jacTe[i]>0.0) ++j; }
else if(lb && p[i]==lb[i]){ ++numactive; if(jacTe[i]<0.0) ++j; }
else if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;
diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */
p_L2+=p[i]*p[i];
}
//p_L2=sqrt(p_L2);
#if 0
if(!(k%100)){
printf("Current estimate: ");
for(i=0; i<m; ++i)
printf("%.9g ", p[i]);
printf("-- errors %.9g %0.9g, #active %d [%d]\n", jacTe_inf, p_eL2, numactive, j);
}
#endif
/* check for convergence */
if(j==numactive && (jacTe_inf <= eps1)){
Dp_L2=0.0; /* no increment for p in this case */
stop=1;
break;
}
/* compute initial damping factor */
if(k==0){
if(!lb && !ub){ /* no bounds */
for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */
mu=tau*tmp;
}
else
mu=LM_CNST(0.5)*tau*p_eL2; /* use Kanzow's starting mu */
}
/* determine increment using a combination of adaptive damping, line search and projected gradient search */
while(1){
/* augment normal equations */
for(i=0; i<m; ++i)
jacTjac[i*m+i]+=mu;
/* solve augmented equations */
#ifdef HAVE_LAPACK
/* 5 alternatives are available: LU, Cholesky, 2 variants of QR decomposition and SVD.
* Cholesky is the fastest but might be inaccurate; QR is slower but more accurate;
* SVD is the slowest but most accurate; LU offers a tradeoff between accuracy and speed
*/
issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_LU;
//issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_CHOL;
//issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_QR;
//issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); linsolver=AX_EQ_B_QRLS;
//issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_SVD;
#else
/* use the LU included with levmar */
issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_LU;
#endif /* HAVE_LAPACK */
if(issolved){
for(i=0; i<m; ++i)
pDp[i]=p[i] + Dp[i];
/* compute p's new estimate and ||Dp||^2 */
BOXPROJECT(pDp, lb, ub, m); /* project to feasible set */
for(i=0, Dp_L2=0.0; i<m; ++i){
Dp[i]=tmp=pDp[i]-p[i];
Dp_L2+=tmp*tmp;
}
//Dp_L2=sqrt(Dp_L2);
if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
stop=2;
break;
}
if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */
stop=4;
break;
}
(*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + Dp */
/* ### hx=x-hx, pDp_eL2=||hx|| */
#if 1
pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);
#else
for(i=0, pDp_eL2=0.0; i<n; ++i){ /* compute ||e(pDp)||_2 */
hx[i]=tmp=x[i]-hx[i];
pDp_eL2+=tmp*tmp;
}
#endif
if(!LM_FINITE(pDp_eL2)){
stop=7;
break;
}
if(pDp_eL2<=gamma_sq*p_eL2){
for(i=0, dL=0.0; i<m; ++i)
dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);
#if 1
if(dL>0.0){
dF=p_eL2-pDp_eL2;
tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));
tmp=LM_CNST(1.0)-tmp*tmp*tmp;
mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );
}
else
mu=(mu>=pDp_eL2)? pDp_eL2 : mu; /* pDp_eL2 is the new pDp_eL2 */
#else
mu=(mu>=pDp_eL2)? pDp_eL2 : mu; /* pDp_eL2 is the new pDp_eL2 */
#endif
nu=2;
for(i=0 ; i<m; ++i) /* update p's estimate */
p[i]=pDp[i];
for(i=0; i<n; ++i) /* update e and ||e||_2 */
e[i]=hx[i];
p_eL2=pDp_eL2;
++nLMsteps;
gprevtaken=0;
break;
}
}
else{
/* the augmented linear system could not be solved, increase mu */
mu*=nu;
nu2=nu<<1; // 2*nu;
if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */
stop=5;
break;
}
nu=nu2;
for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
jacTjac[i*m+i]=diag_jacTjac[i];
continue; /* solve again with increased nu */
}
/* if this point is reached, the LM step did not reduce the error;
* see if it is a descent direction
*/
/* negate jacTe (i.e. g) & compute g^T * Dp */
for(i=0, jacTeDp=0.0; i<m; ++i){
jacTe[i]=-jacTe[i];
jacTeDp+=jacTe[i]*Dp[i];
}
if(jacTeDp<=-rho*pow(Dp_L2, _POW_/LM_CNST(2.0))){
/* Dp is a descent direction; do a line search along it */
int mxtake, iretcd;
LM_REAL stepmx;
tmp=(LM_REAL)sqrt(p_L2); stepmx=LM_CNST(1e3)*( (tmp>=LM_CNST(1.0))? tmp : LM_CNST(1.0) );
#if 1
/* use Schnabel's backtracking line search; it requires fewer "func" evaluations */
LNSRCH(m, p, p_eL2, jacTe, Dp, alpha, pDp, &pDp_eL2, func, fstate,
&mxtake, &iretcd, stepmx, steptl, NULL); /* NOTE: LNSRCH() updates hx */
if(iretcd!=0) goto gradproj; /* rather inelegant but effective way to handle LNSRCH() failures... */
#else
/* use the simpler (but slower!) line search described by Kanzow et al */
for(t=tini; t>tmin; t*=beta){
for(i=0; i<m; ++i){
pDp[i]=p[i] + t*Dp[i];
//pDp[i]=__MEDIAN3(lb[i], pDp[i], ub[i]); /* project to feasible set */
}
(*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + t*Dp */
for(i=0, pDp_eL2=0.0; i<n; ++i){ /* compute ||e(pDp)||_2 */
hx[i]=tmp=x[i]-hx[i];
pDp_eL2+=tmp*tmp;
}
if(!LM_FINITE(pDp_eL2)) goto gradproj; /* treat as line search failure */
//if(LM_CNST(0.5)*pDp_eL2<=LM_CNST(0.5)*p_eL2 + t*alpha*jacTeDp) break;
if(pDp_eL2<=p_eL2 + LM_CNST(2.0)*t*alpha*jacTeDp) break;
}
#endif
++nLSsteps;
gprevtaken=0;
/* NOTE: new estimate for p is in pDp, associated error in hx and its norm in pDp_eL2.
* These values are used below to update their corresponding variables
*/
}
else{
gradproj: /* Note that this point can also be reached via a goto when LNSRCH() fails */
/* jacTe is a descent direction; make a projected gradient step */
/* if the previous step was along the gradient descent, try to use the t employed in that step */
/* compute ||g|| */
for(i=0, tmp=0.0; i<m; ++i)
tmp+=jacTe[i]*jacTe[i];
tmp=(LM_REAL)sqrt(tmp);
tmp=LM_CNST(100.0)/(LM_CNST(1.0)+tmp);
t0=(tmp<=tini)? tmp : tini; /* guard against poor scaling & large steps; see (3.50) in C.T. Kelley's book */
for(t=(gprevtaken)? t : t0; t>tming; t*=beta){
for(i=0; i<m; ++i)
pDp[i]=p[i] - t*jacTe[i];
BOXPROJECT(pDp, lb, ub, m); /* project to feasible set */
for(i=0; i<m; ++i)
Dp[i]=pDp[i]-p[i];
(*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p - t*g */
/* compute ||e(pDp)||_2 */
/* ### hx=x-hx, pDp_eL2=||hx|| */
#if 1
pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);
#else
for(i=0, pDp_eL2=0.0; i<n; ++i){
hx[i]=tmp=x[i]-hx[i];
pDp_eL2+=tmp*tmp;
}
#endif
if(!LM_FINITE(pDp_eL2)){
stop=7;
goto breaknested;
}
for(i=0, tmp=0.0; i<m; ++i) /* compute ||g^T * Dp|| */
tmp+=jacTe[i]*Dp[i];
if(gprevtaken && pDp_eL2<=p_eL2 + LM_CNST(2.0)*LM_CNST(0.99999)*tmp){ /* starting t too small */
t=t0;
gprevtaken=0;
continue;
}
//if(LM_CNST(0.5)*pDp_eL2<=LM_CNST(0.5)*p_eL2 + alpha*tmp) break;
if(pDp_eL2<=p_eL2 + LM_CNST(2.0)*alpha*tmp) break;
}
++nPGsteps;
gprevtaken=1;
/* NOTE: new estimate for p is in pDp, associated error in hx and its norm in pDp_eL2 */
}
/* update using computed values */
for(i=0, Dp_L2=0.0; i<m; ++i){
tmp=pDp[i]-p[i];
Dp_L2+=tmp*tmp;
}
//Dp_L2=sqrt(Dp_L2);
if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
stop=2;
break;
}
for(i=0 ; i<m; ++i) /* update p's estimate */
p[i]=pDp[i];
for(i=0; i<n; ++i) /* update e and ||e||_2 */
e[i]=hx[i];
p_eL2=pDp_eL2;
break;
} /* inner loop */
}
breaknested: /* NOTE: this point is also reached via an explicit goto! */
if(k>=itmax) stop=3;
for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
jacTjac[i*m+i]=diag_jacTjac[i];
if(info){
info[0]=init_p_eL2;
info[1]=p_eL2;
info[2]=jacTe_inf;
info[3]=Dp_L2;
for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];
info[4]=mu/tmp;
info[5]=(LM_REAL)k;
info[6]=(LM_REAL)stop;
info[7]=(LM_REAL)nfev;
info[8]=(LM_REAL)njev;
}
/* covariance matrix */
if(covar){
LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);
}
if(freework) free(work);
#ifdef LINSOLVERS_RETAIN_MEMORY
if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);
#endif
#if 0
printf("%d LM steps, %d line search, %d projected gradient\n", nLMsteps, nLSsteps, nPGsteps);
#endif
return (stop!=4 && stop!=7)? k : LM_ERROR;
}
/* following struct & LMBC_DIF_XXX functions won't be necessary if a true secant
* version of LEVMAR_BC_DIF() is implemented...
*/
struct LMBC_DIF_DATA{
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
LM_REAL *hx, *hxx;
void *adata;
LM_REAL delta;
};
void LMBC_DIF_FUNC(LM_REAL *p, LM_REAL *hx, int m, int n, void *data)
{
struct LMBC_DIF_DATA *dta=(struct LMBC_DIF_DATA *)data;
/* call user-supplied function passing it the user-supplied data */
(*(dta->func))(p, hx, m, n, dta->adata);
}
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;
/* 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);
}
/* 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
*/
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;
//fprintf(stderr, RCAT("\nWarning: current implementation of ", LEVMAR_BC_DIF) "() does not use a secant approach!\n\n");
data.func=func;
data.hx=(LM_REAL *)malloc(2*n*sizeof(LM_REAL)); /* allocate a big chunk in one step */
if(!data.hx){
fprintf(stderr, LCAT(LEVMAR_BC_DIF, "(): memory allocation request failed\n"));
exit(1);
}
data.hxx=data.hx+n;
data.adata=adata;
data.delta=(opts)? FABS(opts[4]) : (LM_REAL)LM_DIFF_DELTA; // no central differences here...
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 */
info[7]+=info[8]*(m+1); /* each Jacobian evaluation costs m+1 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

87
src/external/levmar-2.3/lmblec.c vendored
View File

@ -1,87 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// Levenberg - Marquardt non-linear minimization algorithm
// Copyright (C) 2004-06 Manolis Lourakis (lourakis at ics forth gr)
// Institute of Computer Science, Foundation for Research & Technology - Hellas
// Heraklion, Crete, Greece.
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
/////////////////////////////////////////////////////////////////////////////////
/********************************************************************************
* combined box and linear equation constraints Levenberg-Marquardt nonlinear
* minimization. The same core code is used with appropriate #defines to derive
* single and double precision versions, see also lmblec_core.c
********************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include "lm.h"
#include "misc.h"
#ifndef HAVE_LAPACK
#ifdef _MSC_VER
#pragma message("Combined box and linearly constrained optimization requires LAPACK and was not compiled!")
#else
#warning Combined box and linearly constrained optimization requires LAPACK and was not compiled!
#endif // _MSC_VER
#else // LAPACK present
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
#endif
#ifdef LM_SNGL_PREC
/* single precision (float) definitions */
#define LM_REAL float
#define LM_PREFIX s
#define LM_REAL_MAX FLT_MAX
#define LM_REAL_MIN -FLT_MAX
#define __SUBCNST(x) x##F
#define LM_CNST(x) __SUBCNST(x) // force substitution
#include "lmblec_core.c" // read in core code
#undef LM_REAL
#undef LM_PREFIX
#undef LM_REAL_MAX
#undef LM_REAL_MIN
#undef __SUBCNST
#undef LM_CNST
#endif /* LM_SNGL_PREC */
#ifdef LM_DBL_PREC
/* double precision definitions */
#define LM_REAL double
#define LM_PREFIX d
#define LM_REAL_MAX DBL_MAX
#define LM_REAL_MIN -DBL_MAX
#define LM_CNST(x) (x)
#include "lmblec_core.c" // read in core code
#undef LM_REAL
#undef LM_PREFIX
#undef LM_REAL_MAX
#undef LM_REAL_MIN
#undef LM_CNST
#endif /* LM_DBL_PREC */
#endif /* HAVE_LAPACK */

393
src/external/levmar-2.3/lmblec_core.c vendored
View File

@ -1,393 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// 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)
#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
struct LMBLEC_DATA{
LM_REAL *x, *lb, *ub, *w;
int *bctype;
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);
void *adata;
};
/* augmented measurements */
static void LMBLEC_FUNC(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata)
{
struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
int nn;
register int i, j, *typ;
register LM_REAL *lb, *ub, *w, tmp;
nn=n-m;
lb=data->lb;
ub=data->ub;
w=data->w;
typ=data->bctype;
(*(data->func))(p, hx, m, nn, data->adata);
for(i=nn, j=0; i<n; ++i, ++j){
switch(typ[j]){
case __BC_INTERVAL__:
tmp=(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(ub[j]-lb[j]);
hx[i]=w[j]*__MAX__(tmp*tmp-LM_CNST(1.0), LM_CNST(0.0));
break;
case __BC_LOW__:
hx[i]=w[j]*__MAX__(lb[j]-p[j], LM_CNST(0.0));
break;
case __BC_HIGH__:
hx[i]=w[j]*__MAX__(p[j]-ub[j], LM_CNST(0.0));
break;
}
}
}
/* augmented Jacobian */
static void LMBLEC_JACF(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata)
{
struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
int nn, *typ;
register int i, j;
register LM_REAL *lb, *ub, *w, tmp;
nn=n-m;
lb=data->lb;
ub=data->ub;
w=data->w;
typ=data->bctype;
(*(data->jacf))(p, jac, m, nn, data->adata);
/* clear all extra rows */
for(i=nn*m; i<n*m; ++i)
jac[i]=0.0;
for(i=nn, j=0; i<n; ++i, ++j){
switch(typ[j]){
case __BC_INTERVAL__:
if(lb[j]<=p[j] && p[j]<=ub[j])
continue; // corresp. jac element already 0
/* out of interval */
tmp=ub[j]-lb[j];
tmp=LM_CNST(4.0)*(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(tmp*tmp);
jac[i*m+j]=w[j]*tmp;
break;
case __BC_LOW__: // (lb[j]<=p[j])? 0.0 : -1.0;
if(lb[j]<=p[j])
continue; // corresp. jac element already 0
/* smaller than lower bound */
jac[i*m+j]=-w[j];
break;
case __BC_HIGH__: // (p[j]<=ub[j])? 0.0 : 1.0;
if(p[j]<=ub[j])
continue; // corresp. jac element already 0
/* greater than upper bound */
jac[i*m+j]=w[j];
break;
}
}
}
/*
* This function seeks the parameter vector p that best describes the measurements
* vector x under box & linear constraints.
* More precisely, given a vector function func : R^m --> R^n with n>=m,
* it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of
* e=x-func(p) is minimized under the constraints lb[i]<=p[i]<=ub[i] and A p=b;
* A is kxm, b kx1. Note that this function DOES NOT check the satisfiability of
* the specified box and linear equation constraints.
* If no lower bound constraint applies for p[i], use -DBL_MAX/-FLT_MAX for lb[i];
* If no upper bound constraint applies for p[i], use DBL_MAX/FLT_MAX for ub[i].
*
* This function requires an analytic Jacobian. In case the latter is unavailable,
* use LEVMAR_BLEC_DIF() bellow
*
* Returns the number of iterations (>=0) if successfull, LM_ERROR if failed
*
* 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
*/
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){
fprintf(stderr, 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;
}
if(!LEVMAR_BOX_CHECK(lb, ub, m)){
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
return LM_ERROR;
}
/* 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){
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
exit(1);
}
for(i=0; i<n; ++i)
data.x[i]=x[i];
for(i=n; i<n+m; ++i)
data.x[i]=0.0;
}
else
data.x=NULL;
data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int));
if(!data.w){
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
exit(1);
}
data.bctype=(int *)(data.w+m);
for(i=0; i<m; ++i){
data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
else data.bctype[i]=__BC_HIGH__;
}
data.lb=lb;
data.ub=ub;
data.func=func;
data.jacf=jacf;
data.adata=adata;
if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DER() is called with non-null info */
ret=LEVMAR_LEC_DER(LMBLEC_FUNC, LMBLEC_JACF, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);
if(data.x) free(data.x);
free(data.w);
return ret;
}
/* Similar to the LEVMAR_BLEC_DER() function above, except that the Jacobian is approximated
* with the aid of finite differences (forward or central, see the comment for the opts argument)
*/
int LEVMAR_BLEC_DIF(
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */
LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */
LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */
int m, /* I: parameter vector dimension (i.e. #unknowns) */
int n, /* I: measurement vector dimension */
LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */
LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */
LM_REAL *A, /* I: constraints matrix, kxm */
LM_REAL *b, /* I: right hand constraints vector, kx1 */
int k, /* I: number of constraints (i.e. A's #rows) */
LM_REAL *wghts, /* mx1 weights for penalty terms, defaults used if NULL */
int itmax, /* I: maximum number of iterations */
LM_REAL opts[5], /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
* scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
* the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
* If \delta<0, the Jacobian is approximated with central differences which are more accurate
* (but slower!) compared to the forward differences employed by default.
*/
LM_REAL info[LM_INFO_SZ],
/* O: information regarding the minimization. Set to NULL if don't care
* info[0]= ||e||_2 at initial p.
* info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
* info[5]= # iterations,
* info[6]=reason for terminating: 1 - stopped by small gradient J^T e
* 2 - stopped by small Dp
* 3 - stopped by itmax
* 4 - singular matrix. Restart from current p with increased mu
* 5 - no further error reduction is possible. Restart with increased mu
* 6 - stopped by small ||e||_2
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
* info[7]= # function evaluations
* info[8]= # Jacobian evaluations
*/
LM_REAL *work, /* working memory at least LM_BLEC_DIF_WORKSZ() reals large, allocated if NULL */
LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
void *adata) /* pointer to possibly additional data, passed uninterpreted to func.
* Set to NULL if not needed
*/
{
struct LMBLEC_DATA data;
int ret;
register int i;
LM_REAL locinfo[LM_INFO_SZ];
if(!LEVMAR_BOX_CHECK(lb, ub, m)){
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
return LM_ERROR;
}
/* 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){
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
exit(1);
}
for(i=0; i<n; ++i)
data.x[i]=x[i];
for(i=n; i<n+m; ++i)
data.x[i]=0.0;
}
else
data.x=NULL;
data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int));
if(!data.w){
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
exit(1);
}
data.bctype=(int *)(data.w+m);
for(i=0; i<m; ++i){
data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
else data.bctype[i]=__BC_HIGH__;
}
data.lb=lb;
data.ub=ub;
data.func=func;
data.jacf=NULL;
data.adata=adata;
if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DIF() is called with non-null info */
ret=LEVMAR_LEC_DIF(LMBLEC_FUNC, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);
if(data.x) free(data.x);
free(data.w);
return ret;
}
/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
#undef LEVMAR_BOX_CHECK
#undef LMBLEC_DATA
#undef LMBLEC_FUNC
#undef LMBLEC_JACF
#undef LEVMAR_FDIF_FORW_JAC_APPROX
#undef LEVMAR_COVAR
#undef LEVMAR_LEC_DER
#undef LEVMAR_LEC_DIF
#undef LEVMAR_BLEC_DER
#undef LEVMAR_BLEC_DIF

1027
src/external/levmar-2.3/lmdemo.c vendored
View File

File diff suppressed because it is too large Load Diff

80
src/external/levmar-2.3/lmlec.c vendored
View File

@ -1,80 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// Levenberg - Marquardt non-linear minimization algorithm
// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)
// Institute of Computer Science, Foundation for Research & Technology - Hellas
// Heraklion, Crete, Greece.
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
/////////////////////////////////////////////////////////////////////////////////
/*******************************************************************************
* Wrappers for linearly constrained Levenberg-Marquardt minimization. The same
* core code is used with appropriate #defines to derive single and double
* precision versions, see also lmlec_core.c
*******************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "lm.h"
#include "misc.h"
#ifndef HAVE_LAPACK
#ifdef _MSC_VER
#pragma message("Linearly constrained optimization requires LAPACK and was not compiled!")
#else
#warning Linearly constrained optimization requires LAPACK and was not compiled!
#endif // _MSC_VER
#else // LAPACK present
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
#endif
#ifdef LM_SNGL_PREC
/* single precision (float) definitions */
#define LM_REAL float
#define LM_PREFIX s
#define __SUBCNST(x) x##F
#define LM_CNST(x) __SUBCNST(x) // force substitution
#include "lmlec_core.c" // read in core code
#undef LM_REAL
#undef LM_PREFIX
#undef __SUBCNST
#undef LM_CNST
#endif /* LM_SNGL_PREC */
#ifdef LM_DBL_PREC
/* double precision definitions */
#define LM_REAL double
#define LM_PREFIX d
#define LM_CNST(x) (x)
#include "lmlec_core.c" // read in core code
#undef LM_REAL
#undef LM_PREFIX
#undef LM_CNST
#endif /* LM_DBL_PREC */
#endif /* HAVE_LAPACK */

654
src/external/levmar-2.3/lmlec_core.c vendored
View File

@ -1,654 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// 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_ADD_PREFIX(geqp3_)
#define ORGQR LM_ADD_PREFIX(orgqr_)
#define TRTRI LM_ADD_PREFIX(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 0 in case of error, A's computed rank if successfull
*
*/
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){
fprintf(stderr, RCAT("matrix of constraints cannot have more rows than columns in", LMLEC_ELIM) "()!\n");
return LM_ERROR;
}
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=jpvt_sz*sizeof(int) + (a_sz + tau_sz + r_sz + worksz + Y_sz)*sizeof(LM_REAL);
buf=(LM_REAL *)malloc(tot_sz); /* allocate a "big" memory chunk at once */
if(!buf){
fprintf(stderr, RCAT("Memory allocation request failed in ", LMLEC_ELIM) "()\n");
exit(1);
}
a=(LM_REAL *)buf;
jpvt=(int *)(a+a_sz);
tau=(LM_REAL *)(jpvt + jpvt_sz);
r=tau+tau_sz;
work=r+r_sz;
if(!Y) Y=work+worksz;
/* copy input array so that LAPACK won't destroy it. Note that copying is
* done in row-major order, which equals A^T in column-major
*/
for(i=0; i<tm*tn; ++i)
a[i]=A[i];
/* clear jpvt */
for(i=0; i<jpvt_sz; ++i) jpvt[i]=0;
/* rank revealing QR decomposition of A^T*/
GEQP3((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, (int *)&worksz, &info);
//dgeqpf_((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, &info);
/* error checking */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQP3) " in ", LMLEC_ELIM) "()\n", -info);
exit(1);
}
else if(info>0){
fprintf(stderr, RCAT(RCAT("unknown LAPACK error (%d) for ", GEQP3) " in ", LMLEC_ELIM) "()\n", info);
free(buf);
return 0;
}
}
/* the upper triangular part of a now contains the upper triangle of the unpermuted R */
if(eps<0.0){
LM_REAL aux;
/* compute machine epsilon. DBL_EPSILON should do also */
for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
;
eps*=LM_CNST(2.0);
}
tmp=tm*LM_CNST(10.0)*eps*FABS(a[0]); // threshold. tm is max(tm, tn)
tmp=(tmp>LM_CNST(1E-12))? tmp : LM_CNST(1E-12); // ensure that threshold is not too small
/* compute A^T's numerical rank by counting the non-zeros in R's diagonal */
for(i=rank=0; i<mintmn; ++i)
if(a[i*(tm+1)]>tmp || a[i*(tm+1)]<-tmp) ++rank; /* loop across R's diagonal elements */
else break; /* diagonal is arranged in absolute decreasing order */
if(rank<tn){
fprintf(stderr, RCAT("\nConstraints matrix in ", LMLEC_ELIM) "() is not of full row rank (i.e. %d < %d)!\n"
"Make sure that you do not specify redundant or inconsistent constraints.\n\n", rank, tn);
//exit(1);
free(buf);
return LM_ERROR;
}
/* compute the permuted inverse transpose of R */
/* first, copy R from the upper triangular part of a to r. R is rank x rank */
for(j=0; j<rank; ++j){
for(i=0; i<=j; ++i)
r[i+j*rank]=a[i+j*tm];
for(i=j+1; i<rank; ++i)
r[i+j*rank]=0.0; // lower part is zero
}
/* compute the inverse */
TRTRI("U", "N", (int *)&rank, r, (int *)&rank, &info);
/* error checking */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRI) " in ", LMLEC_ELIM) "()\n", -info);
exit(1);
}
else if(info>0){
fprintf(stderr, RCAT(RCAT("A(%d, %d) is exactly zero for ", TRTRI) " (singular matrix) in ", LMLEC_ELIM) "()\n", info, info);
free(buf);
return 0;
}
}
/* then, transpose r in place */
for(i=0; i<rank; ++i)
for(j=i+1; j<rank; ++j){
tmp=r[i+j*rank];
k=j+i*rank;
r[i+j*rank]=r[k];
r[k]=tmp;
}
/* finally, permute R^-T using Y as intermediate storage */
for(j=0; j<rank; ++j)
for(i=0, k=jpvt[j]-1; i<rank; ++i)
Y[i+k*rank]=r[i+j*rank];
for(i=0; i<rank*rank; ++i) // copy back to r
r[i]=Y[i];
/* resize a to be tm x tm, filling with zeroes */
for(i=tm*tn; i<tm*tm; ++i)
a[i]=0.0;
/* compute Q in a as the product of elementary reflectors. Q is tm x tm */
ORGQR((int *)&tm, (int *)&tm, (int *)&mintmn, a, (int *)&tm, tau, work, &worksz, &info);
/* error checking */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", ORGQR) " in ", LMLEC_ELIM) "()\n", -info);
exit(1);
}
else if(info>0){
fprintf(stderr, RCAT(RCAT("unknown LAPACK error (%d) for ", ORGQR) " in ", LMLEC_ELIM) "()\n", info);
free(buf);
return 0;
}
}
/* compute Y=Q_1*R^-T*P^T. Y is tm x rank */
for(i=0; i<tm; ++i)
for(j=0; j<rank; ++j){
for(k=0, tmp=0.0; k<rank; ++k)
tmp+=a[i+k*tm]*r[k+j*rank];
Y[i*rank+j]=tmp;
}
if(b && c){
/* compute c=Y*b */
for(i=0; i<tm; ++i){
for(j=0, tmp=0.0; j<rank; ++j)
tmp+=Y[i*rank+j]*b[j];
c[i]=tmp;
}
}
/* copy Q_2 into Z. Z is tm x (tm-rank) */
for(j=0; j<tm-rank; ++j)
for(i=0, k=j+rank; i<tm; ++i)
Z[i*(tm-rank)+j]=a[i+k*tm];
free(buf);
return rank;
}
/* constrained measurements: given pp, compute the measurements at c + Z*pp */
static void LMLEC_FUNC(LM_REAL *pp, LM_REAL *hx, int mm, int n, void *adata)
{
struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;
int m;
register int i, j;
register LM_REAL sum;
LM_REAL *c, *Z, *p, *Zimm;
m=mm+data->ncnstr;
c=data->c;
Z=data->Z;
p=data->p;
/* p=c + Z*pp */
for(i=0; i<m; ++i){
Zimm=Z+i*mm;
for(j=0, sum=c[i]; j<mm; ++j)
sum+=Zimm[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];
p[i]=sum;
}
(*(data->func))(p, hx, m, n, data->adata);
}
/* constrained Jacobian: given pp, compute the Jacobian at c + Z*pp
* Using the chain rule, the Jacobian with respect to pp equals the
* product of the Jacobian with respect to p (at c + Z*pp) times Z
*/
static void LMLEC_JACF(LM_REAL *pp, LM_REAL *jacjac, int mm, int n, void *adata)
{
struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;
int m;
register int i, j, l;
register LM_REAL sum, *aux1, *aux2;
LM_REAL *c, *Z, *p, *jac;
m=mm+data->ncnstr;
c=data->c;
Z=data->Z;
p=data->p;
jac=data->jac;
/* p=c + Z*pp */
for(i=0; i<m; ++i){
aux1=Z+i*mm;
for(j=0, sum=c[i]; j<mm; ++j)
sum+=aux1[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];
p[i]=sum;
}
(*(data->jacf))(p, jac, m, n, data->adata);
/* compute jac*Z in jacjac */
if(n*m<=__BLOCKSZ__SQ){ // this is a small problem
/* This is the straightforward way to compute jac*Z. However, due to
* its noncontinuous memory access pattern, it incures many cache misses when
* applied to large minimization problems (i.e. problems involving a large
* number of free variables and measurements), in which jac is too large to
* fit in the L1 cache. For such problems, a cache-efficient blocking scheme
* is preferable. On the other hand, the straightforward algorithm is faster
* on small problems since in this case it avoids the overheads of blocking.
*/
for(i=0; i<n; ++i){
aux1=jac+i*m;
aux2=jacjac+i*mm;
for(j=0; j<mm; ++j){
for(l=0, sum=0.0; l<m; ++l)
sum+=aux1[l]*Z[l*mm+j]; // sum+=jac[i*m+l]*Z[l*mm+j];
aux2[j]=sum; // jacjac[i*mm+j]=sum;
}
}
}
else{ // this is a large problem
/* Cache efficient computation of jac*Z based on blocking
*/
#define __MIN__(x, y) (((x)<=(y))? (x) : (y))
register int jj, ll;
for(jj=0; jj<mm; jj+=__BLOCKSZ__){
for(i=0; i<n; ++i){
aux1=jacjac+i*mm;
for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j)
aux1[j]=0.0; //jacjac[i*mm+j]=0.0;
}
for(ll=0; ll<m; ll+=__BLOCKSZ__){
for(i=0; i<n; ++i){
aux1=jacjac+i*mm; aux2=jac+i*m;
for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j){
sum=0.0;
for(l=ll; l<__MIN__(ll+__BLOCKSZ__, m); ++l)
sum+=aux2[l]*Z[l*mm+j]; //jac[i*m+l]*Z[l*mm+j];
aux1[j]+=sum; //jacjac[i*mm+j]+=sum;
}
}
}
}
}
}
#undef __MIN__
/*
* This function is similar to LEVMAR_DER except that the minimization
* is performed subject to the linear constraints A p=b, A is kxm, b kx1
*
* This function requires an analytic Jacobian. In case the latter is unavailable,
* use LEVMAR_LEC_DIF() bellow
*
*/
int LEVMAR_LEC_DER(
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */
void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), /* function to evaluate the Jacobian \part x / \part p */
LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */
LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */
int m, /* I: parameter vector dimension (i.e. #unknowns) */
int n, /* I: measurement vector dimension */
LM_REAL *A, /* I: constraints matrix, kxm */
LM_REAL *b, /* I: right hand constraints vector, kx1 */
int k, /* I: number of constraints (i.e. A's #rows) */
int itmax, /* I: maximum number of iterations */
LM_REAL opts[4], /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,
* stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used
*/
LM_REAL info[LM_INFO_SZ],
/* O: information regarding the minimization. Set to NULL if don't care
* info[0]= ||e||_2 at initial p.
* info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
* info[5]= # iterations,
* info[6]=reason for terminating: 1 - stopped by small gradient J^T e
* 2 - stopped by small Dp
* 3 - stopped by itmax
* 4 - singular matrix. Restart from current p with increased mu
* 5 - no further error reduction is possible. Restart with increased mu
* 6 - stopped by small ||e||_2
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
* info[7]= # function evaluations
* info[8]= # Jacobian evaluations
*/
LM_REAL *work, /* working memory at least LM_LEC_DER_WORKSZ() reals large, allocated if NULL */
LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
void *adata) /* pointer to possibly additional data, passed uninterpreted to func & jacf.
* Set to NULL if not needed
*/
{
struct LMLEC_DATA data;
LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */
int mm, ret;
register int i, j;
register LM_REAL tmp;
LM_REAL locinfo[LM_INFO_SZ];
if(!jacf){
fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_LEC_DER)
RCAT("().\nIf no such function is available, use ", LEVMAR_LEC_DIF) RCAT("() rather than ", LEVMAR_LEC_DER) "()\n");
return LM_ERROR;
}
mm=m-k;
if(n<mm){
fprintf(stderr, LCAT(LEVMAR_LEC_DER, "(): cannot solve a problem with fewer measurements + constraints [%d + %d] than unknowns [%d]\n"), n, k, m);
return LM_ERROR;
}
ptr=(LM_REAL *)malloc((2*m + m*mm + n*m + mm)*sizeof(LM_REAL));
if(!ptr){
fprintf(stderr, LCAT(LEVMAR_LEC_DER, "(): memory allocation request failed\n"));
exit(1);
}
data.p=p;
p0=ptr;
data.c=p0+m;
data.Z=Z=data.c+m;
data.jac=data.Z+m*mm;
pp=data.jac+n*m;
data.ncnstr=k;
data.func=func;
data.jacf=jacf;
data.adata=adata;
ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z
if(ret==LM_ERROR){
free(ptr);
return LM_ERROR;
}
/* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)
* Due to orthogonality, Z^T Z = I and the last equation
* becomes pp=Z^T*(p-c). Also, save the starting p in p0
*/
for(i=0; i<m; ++i){
p0[i]=p[i];
p[i]-=data.c[i];
}
/* Z^T*(p-c) */
for(i=0; i<mm; ++i){
for(j=0, tmp=0.0; j<m; ++j)
tmp+=Z[j*mm+i]*p[j];
pp[i]=tmp;
}
/* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */
for(i=0; i<m; ++i){
Zimm=Z+i*mm;
for(j=0, tmp=data.c[i]; j<mm; ++j)
tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
if(FABS(tmp-p0[i])>LM_CNST(1E-03))
fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DER) "()! [%.10g reset to %.10g]\n",
i, p0[i], tmp);
}
if(!info) info=locinfo; /* make sure that LEVMAR_DER() is called with non-null info */
/* note that covariance computation is not requested from LEVMAR_DER() */
ret=LEVMAR_DER(LMLEC_FUNC, LMLEC_JACF, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);
/* p=c + Z*pp */
for(i=0; i<m; ++i){
Zimm=Z+i*mm;
for(j=0, tmp=data.c[i]; j<mm; ++j)
tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
p[i]=tmp;
}
/* compute the covariance from the Jacobian in data.jac */
if(covar){
LEVMAR_TRANS_MAT_MAT_MULT(data.jac, covar, n, m); /* covar = J^T J */
LEVMAR_COVAR(covar, covar, info[1], m, n);
}
free(ptr);
return ret;
}
/* Similar to the LEVMAR_LEC_DER() function above, except that the Jacobian is approximated
* with the aid of finite differences (forward or central, see the comment for the opts argument)
*/
int LEVMAR_LEC_DIF(
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in R^n */
LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */
LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */
int m, /* I: parameter vector dimension (i.e. #unknowns) */
int n, /* I: measurement vector dimension */
LM_REAL *A, /* I: constraints matrix, kxm */
LM_REAL *b, /* I: right hand constraints vector, kx1 */
int k, /* I: number of constraints (i.e. A's #rows) */
int itmax, /* I: maximum number of iterations */
LM_REAL opts[5], /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
* scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
* the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
* If \delta<0, the Jacobian is approximated with central differences which are more accurate
* (but slower!) compared to the forward differences employed by default.
*/
LM_REAL info[LM_INFO_SZ],
/* O: information regarding the minimization. Set to NULL if don't care
* info[0]= ||e||_2 at initial p.
* info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
* info[5]= # iterations,
* info[6]=reason for terminating: 1 - stopped by small gradient J^T e
* 2 - stopped by small Dp
* 3 - stopped by itmax
* 4 - singular matrix. Restart from current p with increased mu
* 5 - no further error reduction is possible. Restart with increased mu
* 6 - stopped by small ||e||_2
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
* info[7]= # function evaluations
* info[8]= # Jacobian evaluations
*/
LM_REAL *work, /* working memory at least LM_LEC_DIF_WORKSZ() reals large, allocated if NULL */
LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
void *adata) /* pointer to possibly additional data, passed uninterpreted to func.
* Set to NULL if not needed
*/
{
struct LMLEC_DATA data;
LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */
int mm, ret;
register int i, j;
register LM_REAL tmp;
LM_REAL locinfo[LM_INFO_SZ];
mm=m-k;
if(n<mm){
fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): cannot solve a problem with fewer measurements + constraints [%d + %d] than unknowns [%d]\n"), n, k, m);
return LM_ERROR;
}
ptr=(LM_REAL *)malloc((2*m + m*mm + mm)*sizeof(LM_REAL));
if(!ptr){
fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));
exit(1);
}
data.p=p;
p0=ptr;
data.c=p0+m;
data.Z=Z=data.c+m;
data.jac=NULL;
pp=data.Z+m*mm;
data.ncnstr=k;
data.func=func;
data.jacf=NULL;
data.adata=adata;
ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z
if(ret==LM_ERROR){
free(ptr);
return LM_ERROR;
}
/* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)
* Due to orthogonality, Z^T Z = I and the last equation
* becomes pp=Z^T*(p-c). Also, save the starting p in p0
*/
for(i=0; i<m; ++i){
p0[i]=p[i];
p[i]-=data.c[i];
}
/* Z^T*(p-c) */
for(i=0; i<mm; ++i){
for(j=0, tmp=0.0; j<m; ++j)
tmp+=Z[j*mm+i]*p[j];
pp[i]=tmp;
}
/* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */
for(i=0; i<m; ++i){
Zimm=Z+i*mm;
for(j=0, tmp=data.c[i]; j<mm; ++j)
tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
if(FABS(tmp-p0[i])>LM_CNST(1E-03))
fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DIF) "()! [%.10g reset to %.10g]\n",
i, p0[i], tmp);
}
if(!info) info=locinfo; /* make sure that LEVMAR_DIF() is called with non-null info */
/* note that covariance computation is not requested from LEVMAR_DIF() */
ret=LEVMAR_DIF(LMLEC_FUNC, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);
/* p=c + Z*pp */
for(i=0; i<m; ++i){
Zimm=Z+i*mm;
for(j=0, tmp=data.c[i]; j<mm; ++j)
tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
p[i]=tmp;
}
/* compute the Jacobian with finite differences and use it to estimate the covariance */
if(covar){
LM_REAL *hx, *wrk, *jac;
hx=(LM_REAL *)malloc((2*n+n*m)*sizeof(LM_REAL));
if(!hx){
fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));
exit(1);
}
wrk=hx+n;
jac=wrk+n;
(*func)(p, hx, m, n, adata); /* evaluate function at p */
LEVMAR_FDIF_FORW_JAC_APPROX(func, p, hx, wrk, (LM_REAL)LM_DIFF_DELTA, jac, m, n, adata); /* compute the Jacobian at p */
LEVMAR_TRANS_MAT_MAT_MULT(jac, covar, n, m); /* covar = J^T J */
LEVMAR_COVAR(covar, covar, info[1], m, n);
free(hx);
}
free(ptr);
return ret;
}
/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
#undef LMLEC_DATA
#undef LMLEC_ELIM
#undef LMLEC_FUNC
#undef LMLEC_JACF
#undef LEVMAR_FDIF_FORW_JAC_APPROX
#undef LEVMAR_COVAR
#undef LEVMAR_TRANS_MAT_MAT_MULT
#undef LEVMAR_LEC_DER
#undef LEVMAR_LEC_DIF
#undef LEVMAR_DER
#undef LEVMAR_DIF
#undef GEQP3
#undef ORGQR
#undef TRTRI

30
src/external/levmar-2.3/matlab/Makefile vendored
View File

@ -1,30 +0,0 @@
#
# Unix/Linux Makefile for MATLAB interface to levmar
#
MEX=mex
MEXCFLAGS=-I.. -O #-g
# WHEN USING LAPACK, CHANGE THE NEXT TWO LINES TO WHERE YOUR COMPILED LAPACK/BLAS & F2C LIBS ARE!
LAPACKBLASLIBS_PATH=/usr/lib
F2CLIBS_PATH=/usr/local/lib
# I had to specify the absolute path to the libs, otherwise mex linked against their dynamic versions...
INTFACESRCS=levmar.c
LAPACKLIBS=$(LAPACKBLASLIBS_PATH)/liblapack.a $(LAPACKBLASLIBS_PATH)/libblas.a $(F2CLIBS_PATH)/libf2c.a
# On systems with a FORTRAN (not f2c'ed) version of LAPACK, libf2c.a is
# not necessary; on others, libf2c.a comes in two parts: libF77.a and libI77.a
LIBS=$(LAPACKLIBS)
dummy: $(INTFACESRCS)
$(MEX) $(MEXCFLAGS) $(INTFACESRCS) ../liblevmar.a $(LIBS)
clean:
@rm -f levmar.mexglx
depend:
makedepend -f Makefile $(INTFACESRCS)
# DO NOT DELETE THIS LINE -- make depend depends on it.

26
src/external/levmar-2.3/matlab/Makefile.w32 vendored
View File

@ -1,26 +0,0 @@
#
# Windows Makefile for MATLAB interface to levmar
#
MEX=mex
MEXCFLAGS=-I.. -O #-g
# WHEN USING LAPACK, CHANGE THE NEXT TWO LINES TO WHERE YOUR COMPILED LAPACK/BLAS & F2C LIBS ARE!
LAPACKBLASLIBS_PATH=C:\src\lib
F2CLIBS_PATH=$(LAPACKBLASLIBS_PATH) # define appropriately if not identical to LAPACKBLASLIBS_PATH
INTFACESRCS=levmar.c
LAPACKLIBS=$(LAPACKBLASLIBS_PATH)/clapack.lib $(LAPACKBLASLIBS_PATH)/blas.lib $(F2CLIBS_PATH)/libF77.lib $(F2CLIBS_PATH)/libI77.lib
LIBS=$(LAPACKLIBS)
dummy: $(INTFACESRCS)
$(MEX) $(MEXCFLAGS) $(INTFACESRCS) ../levmar.lib $(LIBS)
clean:
-del levmar.mexw32
depend:
makedepend -f Makefile $(INTFACESRCS)
# DO NOT DELETE THIS LINE -- make depend depends on it.

34
src/external/levmar-2.3/matlab/README.txt vendored
View File

@ -1,34 +0,0 @@
This directory contains a matlab MEX interface to levmar. This interface
has been tested with Matlab v. 6.5 R13 under linux and v. 7.4 R2007 under Windows.
FILES
The following files are included:
levmar.c: C MEX-file for levmar
Makefile: UNIX makefile for compiling levmar.c using mex
Makefile.w32: Windows makefile for compiling levmar.c using mex
levmar.m: Documentation for the MEX interface
lmdemo.m: Demonstration of using the MEX interface; run as matlab < lmdemo.m
*.m: Matlab functions implementing various objective functions and their Jacobians.
For instance, meyer.m implements the objective function for Meyer's (reformulated)
problem and jacmeyer.m implements its Jacobian.
COMPILING
Use the provided Makefile or Makefile.w32, depending on your platform.
Alternatively, levmar.c can be compiled from matlab's prompt with a
command like
mex -DHAVE_LAPACK -I.. -O -L<levmar library dir> -L<blas/lapack libraries dir> levmar.c -llevmar -lclapack -lblas -lf2c
Make sure that you substitute the angle brackets with the correct paths to
the levmar and the blas/lapack directories. Also, on certain systems,
-lf2c should be changed to -llibF77 -llibI77
If your mex compiler has not been configured, the following command should be run first:
mex -setup
TESTING
After compiling, execute lmdemo.m with matlab < lmdemo.m

11
src/external/levmar-2.3/matlab/bt3.m vendored
View File

@ -1,11 +0,0 @@
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

8
src/external/levmar-2.3/matlab/expfit.m vendored
View File

@ -1,8 +0,0 @@
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

6
src/external/levmar-2.3/matlab/hs01.m vendored
View File

@ -1,6 +0,0 @@
function x = hs01(p)
n=2;
t=p(1)*p(1);
x(1)=10.0*(p(2)-t);
x(2)=1.0-p(1);

13
src/external/levmar-2.3/matlab/jacbt3.m vendored
View File

@ -1,13 +0,0 @@
function jac = bt3_jac(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

7
src/external/levmar-2.3/matlab/jacexpfit.m vendored
View File

@ -1,7 +0,0 @@
function jac = expfit_jac(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

5
src/external/levmar-2.3/matlab/jachs01.m vendored
View File

@ -1,5 +0,0 @@
function jac = hs01_jac(p)
m=2;
jac(1, 1:m)=[-20.0*p(1), 10.0];
jac(2, 1:m)=[-1.0, 0.0];

10
src/external/levmar-2.3/matlab/jacmeyer.m vendored
View File

@ -1,10 +0,0 @@
function jac = meyer_jac(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

7
src/external/levmar-2.3/matlab/jacmodhs52.m vendored
View File

@ -1,7 +0,0 @@
function jac = modhs52_jac(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];

580
src/external/levmar-2.3/matlab/levmar.c vendored
View File

@ -1,580 +0,0 @@
/* ////////////////////////////////////////////////////////////////////////////////
//
// Matlab MEX file for the Levenberg - Marquardt minimization algorithm
// Copyright (C) 2007 Manolis Lourakis (lourakis at ics forth gr)
// Institute of Computer Science, Foundation for Research & Technology - Hellas
// Heraklion, Crete, Greece.
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//////////////////////////////////////////////////////////////////////////////// */
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <math.h>
#include <string.h>
#include <ctype.h>
#include <lm.h>
#include <mex.h>
/**
#define DEBUG
**/
#ifndef HAVE_LAPACK
#ifdef _MSC_VER
#pragma message("LAPACK not available, certain functionalities cannot be compiled!")
#else
#warning LAPACK not available, certain functionalities cannot be compiled
#endif /* _MSC_VER */
#endif /* HAVE_LAPACK */
#define __MAX__(A, B) ((A)>=(B)? (A) : (B))
#define MIN_UNCONSTRAINED 0
#define MIN_CONSTRAINED_BC 1
#define MIN_CONSTRAINED_LEC 2
#define MIN_CONSTRAINED_BLEC 3
struct mexdata {
/* matlab names of the fitting function & its Jacobian */
char *fname, *jacname;
/* binary flags specifying if input p0 is a row or column vector */
int isrow_p0;
/* rhs args to be passed to matlab. rhs[0] is reserved for
* passing the parameter vector. If present, problem-specific
* data are passed in rhs[1], rhs[2], etc
*/
mxArray **rhs;
int nrhs; /* >= 1 */
};
/* display printf-style error messages in matlab */
static void matlabFmtdErrMsgTxt(char *fmt, ...)
{
char buf[256];
va_list args;
va_start(args, fmt);
vsprintf(buf, fmt, args);
va_end(args);
mexErrMsgTxt(buf);
}
/* display printf-style warning messages in matlab */
static void matlabFmtdWarnMsgTxt(char *fmt, ...)
{
char buf[256];
va_list args;
va_start(args, fmt);
vsprintf(buf, fmt, args);
va_end(args);
mexWarnMsgTxt(buf);
}
static void func(double *p, double *hx, int m, int n, void *adata)
{
mxArray *lhs[1];
double *mp, *mx;
register int i;
struct mexdata *dat=(struct mexdata *)adata;
/* prepare to call matlab */
mp=mxGetPr(dat->rhs[0]);
for(i=0; i<m; ++i)
mp[i]=p[i];
/* invoke matlab */
mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->fname);
/* copy back results & cleanup */
mx=mxGetPr(lhs[0]);
for(i=0; i<n; ++i)
hx[i]=mx[i];
/* delete the matrix created by matlab */
mxDestroyArray(lhs[0]);
}
static void jacfunc(double *p, double *j, int m, int n, void *adata)
{
mxArray *lhs[1];
double *mp;
double *mj;
register int i, k;
struct mexdata *dat=(struct mexdata *)adata;
/* prepare to call matlab */
mp=mxGetPr(dat->rhs[0]);
for(i=0; i<m; ++i)
mp[i]=p[i];
/* invoke matlab */
mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->jacname);
/* copy back results & cleanup. Note that the nxm Jacobian
* computed by matlab should be transposed so that
* levmar gets it in row major, as expected
*/
mj=mxGetPr(lhs[0]);
for(i=0; i<n; ++i)
for(k=0; k<m; ++k)
j[i*m+k]=mj[i+k*n];
/* delete the matrix created by matlab */
mxDestroyArray(lhs[0]);
}
/* matlab matrices are in column-major, this routine converts them to row major for levmar */
static double *getTranspose(mxArray *Am)
{
int m, n;
double *At, *A;
register int i, j;
m=mxGetM(Am);
n=mxGetN(Am);
A=mxGetPr(Am);
At=mxMalloc(m*n*sizeof(double));
for(i=0; i<m; i++)
for(j=0; j<n; j++)
At[i*n+j]=A[i+j*m];
return At;
}
/* check the supplied matlab function and its Jacobian. Returns 1 on error, 0 otherwise */
static int checkFuncAndJacobian(double *p, int m, int n, int havejac, struct mexdata *dat)
{
mxArray *lhs[1];
register int i;
int ret=0;
double *mp;
mexSetTrapFlag(1); /* handle errors in the MEX-file */
mp=mxGetPr(dat->rhs[0]);
for(i=0; i<m; ++i)
mp[i]=p[i];
/* attempt to call the supplied func */
i=mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->fname);
if(i){
fprintf(stderr, "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){
fprintf(stderr, "levmar: '%s' should produce a real vector with %d elements (got %d).\n",
dat->fname, m, __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){
fprintf(stderr, "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){
fprintf(stderr, "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])){
fprintf(stderr, "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);
fprintf(stderr, "LEVMAR: %s analytic Jacobian\n", havejac? "with" : "no");
#endif /* DEBUG */
/* CHECK
if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 && mxGetN(prhs[1])==1))
*/
/** p0 **/
/* the second required argument must be a real row or column vector */
if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 || mxGetN(prhs[1])==1))
mexErrMsgTxt("levmar: p0 must be a real vector.");
p0=mxGetPr(prhs[1]);
/* determine if we have a row or column vector and retrieve its
* size, i.e. the number of parameters
*/
if(mxGetM(prhs[1])==1){
m=mxGetN(prhs[1]);
mdata.isrow_p0=1;
}
else{
m=mxGetM(prhs[1]);
mdata.isrow_p0=0;
}
/* copy input parameter vector to avoid destroying it */
p=mxMalloc(m*sizeof(double));
for(i=0; i<m; ++i)
p[i]=p0[i];
/** x **/
/* the third required argument must be a real row or column vector */
if(!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) || !(mxGetM(prhs[2])==1 || mxGetN(prhs[2])==1))
mexErrMsgTxt("levmar: x must be a real vector.");
x=mxGetPr(prhs[2]);
n=__MAX__(mxGetM(prhs[2]), mxGetN(prhs[2]));
/** itmax **/
/* the fourth required argument must be a scalar */
if(!mxIsDouble(prhs[3]) || mxIsComplex(prhs[3]) || mxGetM(prhs[3])!=1 || mxGetN(prhs[3])!=1)
mexErrMsgTxt("levmar: itmax must be a scalar.");
itmax=(int)mxGetScalar(prhs[3]);
/** opts **/
/* the fifth required argument must be a real row or column vector */
if(!mxIsDouble(prhs[4]) || mxIsComplex(prhs[4]) || (!(mxGetM(prhs[4])==1 || mxGetN(prhs[4])==1) &&
!(mxGetM(prhs[4])==0 && mxGetN(prhs[4])==0)))
mexErrMsgTxt("levmar: opts must be a real vector.");
pdbl=mxGetPr(prhs[4]);
nopts=__MAX__(mxGetM(prhs[4]), mxGetN(prhs[4]));
if(nopts!=0){ /* if opts==[], nothing needs to be done and the defaults are used */
if(nopts>LM_OPTS_SZ)
matlabFmtdErrMsgTxt("levmar: opts must have at most %d elements, got %d.", LM_OPTS_SZ, nopts);
else if(nopts<((havejac)? LM_OPTS_SZ-1 : LM_OPTS_SZ))
matlabFmtdWarnMsgTxt("levmar: only the %d first elements of opts specified, remaining set to defaults.", nopts);
for(i=0; i<nopts; ++i)
opts[i]=pdbl[i];
}
#ifdef DEBUG
else{
fflush(stderr);
fprintf(stderr, "LEVMAR: empty options vector, using defaults\n");
}
#endif /* DEBUG */
/** mintype (optional) **/
/* check whether sixth argument is a string */
if(nrhs>=6 && mxIsChar(prhs[5])==1 && mxGetM(prhs[5])==1){
char *minhowto;
/* examine supplied name */
len=mxGetN(prhs[5])+1;
minhowto=mxCalloc(len, sizeof(char));
status=mxGetString(prhs[5], minhowto, len);
if(status!=0)
mexErrMsgTxt("levmar: not enough space. String is truncated.");
for(i=0; minhowto[i]; ++i)
minhowto[i]=tolower(minhowto[i]);
if(!strncmp(minhowto, "unc", 3)) mintype=MIN_UNCONSTRAINED;
else if(!strncmp(minhowto, "bc", 2)) mintype=MIN_CONSTRAINED_BC;
else if(!strncmp(minhowto, "lec", 3)) mintype=MIN_CONSTRAINED_LEC;
else if(!strncmp(minhowto, "blec", 4)) mintype=MIN_CONSTRAINED_BLEC;
else matlabFmtdErrMsgTxt("levmar: unknown minimization type '%s'.", minhowto);
mxFree(minhowto);
++prhs;
--nrhs;
}
else
mintype=MIN_UNCONSTRAINED;
if(mintype==MIN_UNCONSTRAINED) goto extraargs;
/* arguments below this point are optional and their presence depends
* upon the minimization type determined above
*/
/** lb, ub **/
if(nrhs>=7 && (mintype==MIN_CONSTRAINED_BC || mintype==MIN_CONSTRAINED_BLEC)){
/* check if the next two arguments are real row or column vectors */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){
if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
if((i=__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5])))!=m)
matlabFmtdErrMsgTxt("levmar: lb must have %d elements, got %d.", m, i);
if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=m)
matlabFmtdErrMsgTxt("levmar: ub must have %d elements, got %d.", m, i);
lb=mxGetPr(prhs[5]);
ub=mxGetPr(prhs[6]);
prhs+=2;
nrhs-=2;
}
}
}
/** A, b **/
if(nrhs>=7 && (mintype==MIN_CONSTRAINED_LEC || mintype==MIN_CONSTRAINED_BLEC)){
/* check if the next two arguments are a real matrix and a real row or column vector */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && mxGetM(prhs[5])>=1 && mxGetN(prhs[5])>=1){
if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
if((i=mxGetN(prhs[5]))!=m)
matlabFmtdErrMsgTxt("levmar: A must have %d columns, got %d.", m, i);
if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=(Arows=mxGetM(prhs[5])))
matlabFmtdErrMsgTxt("levmar: b must have %d elements, got %d.", Arows, i);
At=prhs[5];
b=mxGetPr(prhs[6]);
A=getTranspose(At);
prhs+=2;
nrhs-=2;
}
}
}
/* wghts */
/* check if we have a weights vector */
if(nrhs>=6 && mintype==MIN_CONSTRAINED_BLEC){ /* only check if we have seen both box & linear constraints */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){
if(__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5]))==m){
wghts=mxGetPr(prhs[5]);
++prhs;
--nrhs;
}
}
}
/* arguments below this point are assumed to be extra arguments passed
* to every invocation of the fitting function and its Jacobian
*/
extraargs:
/* handle any extra args and allocate memory for
* passing the current parameter estimate to matlab
*/
nextra=nrhs-5;
mdata.nrhs=nextra+1;
mdata.rhs=(mxArray **)mxMalloc(mdata.nrhs*sizeof(mxArray *));
for(i=0; i<nextra; ++i)
mdata.rhs[i+1]=(mxArray *)prhs[nrhs-nextra+i]; /* discard 'const' modifier */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: %d extra args\n", nextra);
#endif /* DEBUG */
if(mdata.isrow_p0){ /* row vector */
mdata.rhs[0]=mxCreateDoubleMatrix(1, m, mxREAL);
/*
mxSetM(mdata.rhs[0], 1);
mxSetN(mdata.rhs[0], m);
*/
}
else{ /* column vector */
mdata.rhs[0]=mxCreateDoubleMatrix(m, 1, mxREAL);
/*
mxSetM(mdata.rhs[0], m);
mxSetN(mdata.rhs[0], 1);
*/
}
/* ensure that the supplied function & Jacobian are as expected */
if(checkFuncAndJacobian(p, m, n, havejac, &mdata)){
status=LM_ERROR;
goto cleanup;
}
if(nlhs>3) /* covariance output required */
covar=mxMalloc(m*m*sizeof(double));
/* invoke levmar */
if(!lb && !ub){
if(!A && !b){ /* no constraints */
if(havejac)
status=dlevmar_der(func, jacfunc, p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_dif(func, p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "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);
fprintf(stderr, "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);
fprintf(stderr, "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);
fprintf(stderr, "LEVMAR: calling dlevmar_blec_der()/dlevmar_blec_dif()\n");
#endif /* DEBUG */
}
}
#ifdef DEBUG
fflush(stderr);
printf("LEVMAR: minimization returned %d in %g iter, reason %g\n\tSolution: ", status, info[5], info[6]);
for(i=0; i<m; ++i)
printf("%.7g ", p[i]);
printf("\n\n\tMinimization info:\n\t");
for(i=0; i<LM_INFO_SZ; ++i)
printf("%g ", info[i]);
printf("\n");
#endif /* DEBUG */
/* copy back return results */
/** ret **/
plhs[0]=mxCreateDoubleMatrix(1, 1, mxREAL);
ret=mxGetPr(plhs[0]);
ret[0]=(double)status;
/** popt **/
plhs[1]=(mdata.isrow_p0==1)? mxCreateDoubleMatrix(1, m, mxREAL) : mxCreateDoubleMatrix(m, 1, mxREAL);
pdbl=mxGetPr(plhs[1]);
for(i=0; i<m; ++i)
pdbl[i]=p[i];
/** info **/
if(nlhs>2){
plhs[2]=mxCreateDoubleMatrix(1, LM_INFO_SZ, mxREAL);
pdbl=mxGetPr(plhs[2]);
for(i=0; i<LM_INFO_SZ; ++i)
pdbl[i]=info[i];
}
/** covar **/
if(nlhs>3){
plhs[3]=mxCreateDoubleMatrix(m, m, mxREAL);
pdbl=mxGetPr(plhs[3]);
for(i=0; i<m*m; ++i) /* covariance matrices are symmetric, thus no need to transpose! */
pdbl[i]=covar[i];
}
cleanup:
/* cleanup */
mxDestroyArray(mdata.rhs[0]);
if(A) mxFree(A);
mxFree(mdata.fname);
if(havejac) mxFree(mdata.jacname);
mxFree(p);
mxFree(mdata.rhs);
if(covar) mxFree(covar);
if(status==LM_ERROR)
mexWarnMsgTxt("levmar: optimization returned with an error!");
}

71
src/external/levmar-2.3/matlab/levmar.m vendored
View File

@ -1,71 +0,0 @@
function [ret, popt, info, covar]=levmar(fname, jacname, p0, x, itmax, opts, type)
% LEVMAR matlab MEX interface to the levmar non-linear least squares minimization
% library available from http://www.ics.forth.gr/~lourakis/levmar/
%
% levmar can be used in any of the 4 following ways:
% [ret, popt, info, covar]=levmar(fname, jacname, p0, x, itmax, opts, 'unc', ...)
% [ret, popt, info, covar]=levmar(fname, jacname, p0, x, itmax, opts, 'bc', lb, ub, ...)
% [ret, popt, info, covar]=levmar(fname, jacname, p0, x, itmax, opts, 'lec', A, b, ...)
% [ret, popt, info, covar]=levmar(fname, jacname, p0, x, itmax, opts, 'blec', lb, ub, A, b, wghts, ...)
%
% The dots at the end denote any additional, problem specific data that are passed uniterpreted to
% all invocations of fname and jacname, see below for details.
%
% In the following, the word "vector" is meant to imply either a row or a column vector.
%
% required input arguments:
% - fname: String defining the name of a matlab function implementing the function to be minimized.
% fname will be called as fname(p, ...), where p denotes the parameter vector and the dots any
% additional data passed as extra arguments during the invocation of levmar (refer to Meyer's
% problem in lmdemo.m for an example).
%
% - p0: vector of doubles holding the initial parameters estimates.
%
% - x: vector of doubles holding the measurements vector.
%
% - itmax: maximum number of iterations.
%
% - opts: vector of doubles specifying the minimization parameters, as follows:
% opts(1) scale factor for the initial damping factor
% opts(2) stopping threshold for ||J^T e||_inf
% opts(3) stopping threshold for ||Dp||_2
% opts(4) stopping threshold for ||e||_2
% opts(5) step used in finite difference approximation to the Jacobian.
% If an empty vector (i.e. []) is specified, defaults are used.
%
% optional input arguments:
% - jacname: String defining the name of matlab function implementing the Jacobian of function fname.
% jacname will be called as jacname(p, ...) where p is again the parameter vector and the dots
% denote any additional data passed as extra arguments to the invocation of levmar. If omitted,
% the Jacobian is approximated with finite differences through repeated invocations of fname.
%
% - type: String defining the minimization type. It should be one of the following:
% 'unc' specifies unconstrained minimization.
% 'bc' specifies minimization subject to box constraints.
% 'lec' specifies minimization subject to linear equation constraints.
% 'blec' specifies minimization subject to box and linear equation constraints.
% If omitted, a default of 'unc' is assumed. Depending on the minimization type, the MEX
% interface will invoke one of dlevmar_XXX, dlevmar_bc_XXX, dlevmar_lec_XXX or dlevmar_blec_XXX
%
% - lb, ub: vectors of doubles specifying lower and upper bounds for p, respectively
%
% - A, b: k x m matrix and k vector specifying linear equation constraints for p, i.e. A*p=b
% A should have full rank.
%
% - wghts: vector of doubles specifying the weights for the penalty terms corresponding to
% the box constraints, see lmblec_core.c for more details. If omitted and a 'blec' type
% minimization is to be carried out, default weights are used.
%
%
% output arguments
% - ret: return value of levmar, corresponding to the number of iterations if successful, -1 otherwise.
%
% - popt: estimated minimizer, i.e. minimized parameters vector.
%
% - info: optional array of doubles, which upon return provides information regarding the minimization.
% See lm_core.c for more details.
%
% - covar: optional covariance matrix corresponding to the estimated minimizer.
%
error('levmar.m is used only for providing documentation to levmar; make sure that levmar.c has been compiled using mex');

103
src/external/levmar-2.3/matlab/lmdemo.m vendored
View File

@ -1,103 +0,0 @@
% Unconstrained minimization
% fitting the exponential model x_i=p(1)*exp(-p(2)*i)+p(3) of expfit.c to noisy measurements obtained with (5.0 0.1 1.0)
p0=[1.0, 0.0, 0.0];
x=[5.8728, 5.4948, 5.0081, 4.5929, 4.3574, 4.1198, 3.6843, 3.3642, 2.9742, 3.0237, 2.7002, 2.8781,...
2.5144, 2.4432, 2.2894, 2.0938, 1.9265, 2.1271, 1.8387, 1.7791, 1.6686, 1.6232, 1.571, 1.6057,...
1.3825, 1.5087, 1.3624, 1.4206, 1.2097, 1.3129, 1.131, 1.306, 1.2008, 1.3469, 1.1837, 1.2102,...
0.96518, 1.2129, 1.2003, 1.0743];
options=[1E-03, 1E-15, 1E-15, 1E-20, 1E-06];
% arg demonstrates additional data passing to expfit/jacexpfit
arg=[40];
[ret, popt, info]=levmar('expfit', 'jacexpfit', p0, x, 200, options, arg);
disp('Exponential model fitting (see also ../expfit.c)');
popt
% Meyer's (reformulated) problem
p0=[8.85, 4.0, 2.5];
x=[];
x(1:4)=[34.780, 28.610, 23.650, 19.630];
x(5:8)=[16.370, 13.720, 11.540, 9.744];
x(9:12)=[8.261, 7.030, 6.005, 5.147];
x(13:16)=[4.427, 3.820, 3.307, 2.872];
options=[1E-03, 1E-15, 1E-15, 1E-20, 1E-06];
% arg1, arg2 demonstrate additional dummy data passing to meyer/jacmeyer
arg1=[17];
arg2=[27];
%[ret, popt, info]=levmar('meyer', 'jacmeyer', p0, x, 200, options, arg1, arg2);
%[ret, popt, info, covar]=levmar('meyer', 'jacmeyer', p0, x, 200, options, arg1, arg2);
[ret, popt, info, covar]=levmar('meyer', p0, x, 200, options, 'unc', arg1, arg2);
disp('Meyer''s (reformulated) problem');
popt
% Linear constraints
% Boggs-Tolle problem 3
p0=[2.0, 2.0, 2.0, 2.0, 2.0];
x=[0.0, 0.0, 0.0, 0.0, 0.0];
options=[1E-03, 1E-15, 1E-15, 1E-20];
adata=[];
A=[1.0, 3.0, 0.0, 0.0, 0.0;
0.0, 0.0, 1.0, 1.0, -2.0;
0.0, 1.0, 0.0, 0.0, -1.0];
b=[0.0, 0.0, 0.0]';
[ret, popt, info, covar]=levmar('bt3', 'jacbt3', p0, x, 200, options, 'lec', A, b, adata);
disp('Boggs-Tolle problem 3');
popt
% Box constraints
% Hock-Schittkowski problem 01
p0=[-2.0, 1.0];
x=[0.0, 0.0];
lb=[-realmax, -1.5];
ub=[realmax, realmax];
options=[1E-03, 1E-15, 1E-15, 1E-20];
[ret, popt, info, covar]=levmar('hs01', 'jachs01', p0, x, 200, options, 'bc', lb, ub);
disp('Hock-Schittkowski problem 01');
popt
% Box and linear constraints
% Hock-Schittkowski modified problem 52
p0=[2.0, 2.0, 2.0, 2.0, 2.0];
x=[0.0, 0.0, 0.0, 0.0];
lb=[-0.09, 0.0, -realmax, -0.2, 0.0];
ub=[realmax, 0.3, 0.25, 0.3, 0.3];
A=[1.0, 3.0, 0.0, 0.0, 0.0;
0.0, 0.0, 1.0, 1.0, -2.0;
0.0, 1.0, 0.0, 0.0, -1.0];
b=[0.0, 0.0, 0.0]';
options=[1E-03, 1E-15, 1E-15, 1E-20];
[ret, popt, info, covar]=levmar('modhs52', 'jacmodhs52', p0, x, 200, options, 'blec', lb, ub, A, b);
disp('Hock-Schittkowski modified problem 52');
popt
% Hock-Schittkowski modified problem 235
p0=[-2.0, 3.0, 1.0];
x=[0.0, 0.0];
lb=[-realmax, 0.1, 0.7];
ub=[realmax, 2.9, realmax];
A=[1.0, 0.0, 1.0;
0.0, 1.0, -4.0];
b=[-1.0, 0.0]';
options=[1E-03, 1E-15, 1E-15, 1E-20];
[ret, popt, info, covar]=levmar('mods235', p0, x, 200, options, 'blec', lb, ub, A, b);
disp('Hock-Schittkowski modified problem 235');
popt

9
src/external/levmar-2.3/matlab/meyer.m vendored
View File

@ -1,9 +0,0 @@
function x = meyer(p, data1, data2)
n=16;
% data1, data2 are actually unused
for i=1:n
ui=0.45+0.05*i;
x(i)=p(1)*exp(10.0*p(2)/(ui+p(3)) - 13.0);
end

7
src/external/levmar-2.3/matlab/modhs52.m vendored
View File

@ -1,7 +0,0 @@
function x = modhs52(p)
n=4;
x(1)=4.0*p(1)-p(2);
x(2)=p(2)+p(3)-2.0;
x(3)=p(4)-1.0;
x(4)=p(5)-1.0;

5
src/external/levmar-2.3/matlab/mods235.m vendored
View File

@ -1,5 +0,0 @@
function x = mods235(p)
n=2;
x(1)=0.1*(p(1)-1.0);
x(2)=p(2)-p(1)*p(1);

70
src/external/levmar-2.3/misc.c vendored
View File

@ -1,70 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// Levenberg - Marquardt non-linear minimization algorithm
// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)
// Institute of Computer Science, Foundation for Research & Technology - Hellas
// Heraklion, Crete, Greece.
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
/////////////////////////////////////////////////////////////////////////////////
/********************************************************************************
* Miscelaneous functions for Levenberg-Marquardt nonlinear minimization. The
* same core code is used with appropriate #defines to derive single and double
* precision versions, see also misc_core.c
********************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include "lm.h"
#include "misc.h"
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
#endif
#ifdef LM_SNGL_PREC
/* single precision (float) definitions */
#define LM_REAL float
#define LM_PREFIX s
#define LM_REAL_EPSILON FLT_EPSILON
#define __SUBCNST(x) x##F
#define LM_CNST(x) __SUBCNST(x) // force substitution
#include "misc_core.c" // read in core code
#undef LM_REAL
#undef LM_PREFIX
#undef LM_REAL_EPSILON
#undef __SUBCNST
#undef LM_CNST
#endif /* LM_SNGL_PREC */
#ifdef LM_DBL_PREC
/* double precision definitions */
#define LM_REAL double
#define LM_PREFIX d
#define LM_REAL_EPSILON DBL_EPSILON
#define LM_CNST(x) (x)
#include "misc_core.c" // read in core code
#undef LM_REAL
#undef LM_PREFIX
#undef LM_REAL_EPSILON
#undef LM_CNST
#endif /* LM_DBL_PREC */

97
src/external/levmar-2.3/misc.h vendored
View File

@ -1,97 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// 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 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 __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_ */

774
src/external/levmar-2.3/misc_core.c vendored
View File

@ -1,774 +0,0 @@
/////////////////////////////////////////////////////////////////////////////////
//
// 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_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_ADD_PREFIX(gesvd_)
#define GESDD LM_ADD_PREFIX(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_ADD_PREFIX(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 speeded up by computing only its
* upper triangular part and copying it to the lower part.
*
* More details on blocking can be found at
* http://www-2.cs.cmu.edu/afs/cs/academic/class/15213-f02/www/R07/section_a/Recitation07-SectionA.pdf
*/
void LEVMAR_TRANS_MAT_MAT_MULT(LM_REAL *a, LM_REAL *b, int n, int m)
{
#ifdef HAVE_LAPACK /* use BLAS matrix multiply */
LM_REAL alpha=LM_CNST(1.0), beta=LM_CNST(0.0);
/* Fool BLAS to compute a^T*a avoiding transposing a: a is equivalent to a^T in column major,
* therefore BLAS computes a*a^T with a and a*a^T in column major, which is equivalent to
* computing a^T*a in row major!
*/
GEMM("N", "T", &m, &m, &n, &alpha, a, &m, a, &m, &beta, b, &m);
#else /* no LAPACK, use blocking-based multiply */
register int i, j, k, jj, kk;
register LM_REAL sum, *bim, *akm;
const int bsize=__BLOCKSZ__;
#define __MIN__(x, y) (((x)<=(y))? (x) : (y))
#define __MAX__(x, y) (((x)>=(y))? (x) : (y))
/* compute upper triangular part using blocking */
for(jj=0; jj<m; jj+=bsize){
for(i=0; i<m; ++i){
bim=b+i*m;
for(j=__MAX__(jj, i); j<__MIN__(jj+bsize, m); ++j)
bim[j]=0.0; //b[i*m+j]=0.0;
}
for(kk=0; kk<n; kk+=bsize){
for(i=0; i<m; ++i){
bim=b+i*m;
for(j=__MAX__(jj, i); j<__MIN__(jj+bsize, m); ++j){
sum=0.0;
for(k=kk; k<__MIN__(kk+bsize, n); ++k){
akm=a+k*m;
sum+=akm[i]*akm[j]; //a[k*m+i]*a[k*m+j];
}
bim[j]+=sum; //b[i*m+j]+=sum;
}
}
}
}
/* copy upper triangular part to the lower one */
for(i=0; i<m; ++i)
for(j=0; j<i; ++j)
b[i*m+j]=b[j*m+i];
#undef __MIN__
#undef __MAX__
#endif /* HAVE_LAPACK */
}
/* forward finite difference approximation to the Jacobian of func */
void LEVMAR_FDIF_FORW_JAC_APPROX(
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
/* function to differentiate */
LM_REAL *p, /* I: current parameter estimate, mx1 */
LM_REAL *hx, /* I: func evaluated at p, i.e. hx=func(p), nx1 */
LM_REAL *hxx, /* W/O: work array for evaluating func(p+delta), nx1 */
LM_REAL delta, /* increment for computing the Jacobian */
LM_REAL *jac, /* O: array for storing approximated Jacobian, nxm */
int m,
int n,
void *adata)
{
register int i, j;
LM_REAL tmp;
register LM_REAL d;
for(j=0; j<m; ++j){
/* determine d=max(1E-04*|p[j]|, delta), see HZ */
d=LM_CNST(1E-04)*p[j]; // force evaluation
d=FABS(d);
if(d<delta)
d=delta;
tmp=p[j];
p[j]+=d;
(*func)(p, hxx, m, n, adata);
p[j]=tmp; /* restore */
d=LM_CNST(1.0)/d; /* invert so that divisions can be carried out faster as multiplications */
for(i=0; i<n; ++i){
jac[i*m+j]=(hxx[i]-hx[i])*d;
}
}
}
/* central finite difference approximation to the Jacobian of func */
void LEVMAR_FDIF_CENT_JAC_APPROX(
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
/* function to differentiate */
LM_REAL *p, /* I: current parameter estimate, mx1 */
LM_REAL *hxm, /* W/O: work array for evaluating func(p-delta), nx1 */
LM_REAL *hxp, /* W/O: work array for evaluating func(p+delta), nx1 */
LM_REAL delta, /* increment for computing the Jacobian */
LM_REAL *jac, /* O: array for storing approximated Jacobian, nxm */
int m,
int n,
void *adata)
{
register int i, j;
LM_REAL tmp;
register LM_REAL d;
for(j=0; j<m; ++j){
/* determine d=max(1E-04*|p[j]|, delta), see HZ */
d=LM_CNST(1E-04)*p[j]; // force evaluation
d=FABS(d);
if(d<delta)
d=delta;
tmp=p[j];
p[j]-=d;
(*func)(p, hxm, m, n, adata);
p[j]=tmp+d;
(*func)(p, hxp, m, n, adata);
p[j]=tmp; /* restore */
d=LM_CNST(0.5)/d; /* invert so that divisions can be carried out faster as multiplications */
for(i=0; i<n; ++i){
jac[i*m+j]=(hxp[i]-hxm[i])*d;
}
}
}
/*
* Check the Jacobian of a n-valued nonlinear function in m variables
* evaluated at a point p, for consistency with the function itself.
*
* Based on fortran77 subroutine CHKDER by
* Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
* Argonne National Laboratory. MINPACK project. March 1980.
*
*
* func points to a function from R^m --> R^n: Given a p in R^m it yields hx in R^n
* jacf points to a function implementing the Jacobian of func, whose correctness
* is to be tested. Given a p in R^m, jacf computes into the nxm matrix j the
* Jacobian of func at p. Note that row i of j corresponds to the gradient of
* the i-th component of func, evaluated at p.
* p is an input array of length m containing the point of evaluation.
* m is the number of variables
* n is the number of functions
* adata points to possible additional data and is passed uninterpreted
* to func, jacf.
* err is an array of length n. On output, err contains measures
* of correctness of the respective gradients. if there is
* no severe loss of significance, then if err[i] is 1.0 the
* i-th gradient is correct, while if err[i] is 0.0 the i-th
* gradient is incorrect. For values of err between 0.0 and 1.0,
* the categorization is less certain. In general, a value of
* err[i] greater than 0.5 indicates that the i-th gradient is
* probably correct, while a value of err[i] less than 0.5
* indicates that the i-th gradient is probably incorrect.
*
*
* The function does not perform reliably if cancellation or
* rounding errors cause a severe loss of significance in the
* evaluation of a function. therefore, none of the components
* of p should be unusually small (in particular, zero) or any
* other value which may cause loss of significance.
*/
void LEVMAR_CHKJAC(
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),
LM_REAL *p, int m, int n, void *adata, LM_REAL *err)
{
LM_REAL factor=LM_CNST(100.0);
LM_REAL one=LM_CNST(1.0);
LM_REAL zero=LM_CNST(0.0);
LM_REAL *fvec, *fjac, *pp, *fvecp, *buf;
register int i, j;
LM_REAL eps, epsf, temp, epsmch;
LM_REAL epslog;
int fvec_sz=n, fjac_sz=n*m, pp_sz=m, fvecp_sz=n;
epsmch=LM_REAL_EPSILON;
eps=(LM_REAL)sqrt(epsmch);
buf=(LM_REAL *)malloc((fvec_sz + fjac_sz + pp_sz + fvecp_sz)*sizeof(LM_REAL));
if(!buf){
fprintf(stderr, LCAT(LEVMAR_CHKJAC, "(): memory allocation request failed\n"));
exit(1);
}
fvec=buf;
fjac=fvec+fvec_sz;
pp=fjac+fjac_sz;
fvecp=pp+pp_sz;
/* compute fvec=func(p) */
(*func)(p, fvec, m, n, adata);
/* compute the Jacobian at p */
(*jacf)(p, fjac, m, n, adata);
/* compute pp */
for(j=0; j<m; ++j){
temp=eps*FABS(p[j]);
if(temp==zero) temp=eps;
pp[j]=p[j]+temp;
}
/* compute fvecp=func(pp) */
(*func)(pp, fvecp, m, n, adata);
epsf=factor*epsmch;
epslog=(LM_REAL)log10(eps);
for(i=0; i<n; ++i)
err[i]=zero;
for(j=0; j<m; ++j){
temp=FABS(p[j]);
if(temp==zero) temp=one;
for(i=0; i<n; ++i)
err[i]+=temp*fjac[i*m+j];
}
for(i=0; i<n; ++i){
temp=one;
if(fvec[i]!=zero && fvecp[i]!=zero && FABS(fvecp[i]-fvec[i])>=epsf*FABS(fvec[i]))
temp=eps*FABS((fvecp[i]-fvec[i])/eps - err[i])/(FABS(fvec[i])+FABS(fvecp[i]));
err[i]=one;
if(temp>epsmch && temp<eps)
err[i]=((LM_REAL)log10(temp) - epslog)/epslog;
if(temp>=eps) err[i]=zero;
}
free(buf);
return;
}
#ifdef HAVE_LAPACK
/*
* This function computes the pseudoinverse of a square matrix A
* into B using SVD. A and B can coincide
*
* The function returns 0 in case of error (e.g. A is singular),
* the rank of A if successfull
*
* 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=16*m; /* more than needed */
iworksz=8*m;
a_sz=m*m;
u_sz=m*m; s_sz=m; vt_sz=m*m;
tot_sz=iworksz*sizeof(int) + (a_sz + u_sz + s_sz + vt_sz + worksz)*sizeof(LM_REAL);
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz);
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", LEVMAR_PSEUDOINVERSE) "() failed!\n");
exit(1);
}
iwork=(int *)buf;
a=(LM_REAL *)(iwork+iworksz);
/* store A (column major!) into a */
for(i=0; i<m; i++)
for(j=0; j<m; j++)
a[i+j*m]=A[i*m+j];
u=a + a_sz;
s=u+u_sz;
vt=s+s_sz;
work=vt+vt_sz;
/* SVD decomposition of A */
GESVD("A", "A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, &info);
//GESDD("A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, iwork, &info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GESVD), "/" GESDD) " in ", LEVMAR_PSEUDOINVERSE) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT("LAPACK error: dgesdd (dbdsdc)/dgesvd (dbdsqr) failed to converge in ", LEVMAR_PSEUDOINVERSE) "() [info=%d]\n", info);
free(buf);
return 0;
}
}
if(eps<0.0){
LM_REAL aux;
/* compute machine epsilon */
for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
;
eps*=LM_CNST(2.0);
}
/* compute the pseudoinverse in B */
for(i=0; i<a_sz; i++) B[i]=0.0; /* initialize to zero */
for(rank=0, thresh=eps*s[0]; rank<m && s[rank]>thresh; rank++){
one_over_denom=LM_CNST(1.0)/s[rank];
for(j=0; j<m; j++)
for(i=0; i<m; i++)
B[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;
}
free(buf);
return rank;
}
#else // no LAPACK
/*
* This function computes the inverse of A in B. A and B can coincide
*
* The function employs LAPACK-free LU decomposition of A to solve m linear
* systems A*B_i=I_i, where B_i and I_i are the i-th columns of B and I.
*
* A and B are mxm
*
* The function returns 0 in case of error,
* 1 if successfull
*
*/
static int LEVMAR_LUINVERSE(LM_REAL *A, LM_REAL *B, int m)
{
void *buf=NULL;
int buf_sz=0;
register int i, j, k, l;
int *idx, maxi=-1, idx_sz, a_sz, x_sz, work_sz, tot_sz;
LM_REAL *a, *x, *work, max, sum, tmp;
/* calculate required memory size */
idx_sz=m;
a_sz=m*m;
x_sz=m;
work_sz=m;
tot_sz=idx_sz*sizeof(int) + (a_sz+x_sz+work_sz)*sizeof(LM_REAL);
buf_sz=tot_sz;
buf=(void *)malloc(tot_sz);
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", LEVMAR_LUINVERSE) "() failed!\n");
exit(1);
}
idx=(int *)buf;
a=(LM_REAL *)(idx + idx_sz);
x=a + a_sz;
work=x + x_sz;
/* avoid destroying A by copying it to a */
for(i=0; i<a_sz; ++i) a[i]=A[i];
/* compute the LU decomposition of a row permutation of matrix a; the permutation itself is saved in idx[] */
for(i=0; i<m; ++i){
max=0.0;
for(j=0; j<m; ++j)
if((tmp=FABS(a[i*m+j]))>max)
max=tmp;
if(max==0.0){
fprintf(stderr, RCAT("Singular matrix A in ", LEVMAR_LUINVERSE) "()!\n");
free(buf);
return 0;
}
work[i]=LM_CNST(1.0)/max;
}
for(j=0; j<m; ++j){
for(i=0; i<j; ++i){
sum=a[i*m+j];
for(k=0; k<i; ++k)
sum-=a[i*m+k]*a[k*m+j];
a[i*m+j]=sum;
}
max=0.0;
for(i=j; i<m; ++i){
sum=a[i*m+j];
for(k=0; k<j; ++k)
sum-=a[i*m+k]*a[k*m+j];
a[i*m+j]=sum;
if((tmp=work[i]*FABS(sum))>=max){
max=tmp;
maxi=i;
}
}
if(j!=maxi){
for(k=0; k<m; ++k){
tmp=a[maxi*m+k];
a[maxi*m+k]=a[j*m+k];
a[j*m+k]=tmp;
}
work[maxi]=work[j];
}
idx[j]=maxi;
if(a[j*m+j]==0.0)
a[j*m+j]=LM_REAL_EPSILON;
if(j!=m-1){
tmp=LM_CNST(1.0)/(a[j*m+j]);
for(i=j+1; i<m; ++i)
a[i*m+j]*=tmp;
}
}
/* The decomposition has now replaced a. Solve the m linear systems using
* forward and back substitution
*/
for(l=0; l<m; ++l){
for(i=0; i<m; ++i) x[i]=0.0;
x[l]=LM_CNST(1.0);
for(i=k=0; i<m; ++i){
j=idx[i];
sum=x[j];
x[j]=x[i];
if(k!=0)
for(j=k-1; j<i; ++j)
sum-=a[i*m+j]*x[j];
else
if(sum!=0.0)
k=i+1;
x[i]=sum;
}
for(i=m-1; i>=0; --i){
sum=x[i];
for(j=i+1; j<m; ++j)
sum-=a[i*m+j]*x[j];
x[i]=sum/a[i*m+i];
}
for(i=0; i<m; ++i)
B[i*m+l]=x[i];
}
free(buf);
return 1;
}
#endif /* HAVE_LAPACK */
/*
* This function computes in C the covariance matrix corresponding to a least
* squares fit. JtJ is the approximate Hessian at the solution (i.e. J^T*J, where
* J is the Jacobian at the solution), sumsq is the sum of squared residuals
* (i.e. goodnes of fit) at the solution, m is the number of parameters (variables)
* and n the number of observations. JtJ can coincide with C.
*
* if JtJ is of full rank, C is computed as sumsq/(n-m)*(JtJ)^-1
* otherwise and if LAPACK is available, C=sumsq/(n-r)*(JtJ)^+
* where r is JtJ's rank and ^+ denotes the pseudoinverse
* The diagonal of C is made up from the estimates of the variances
* of the estimated regression coefficients.
* See the documentation of routine E04YCF from the NAG fortran lib
*
* The function returns the rank of JtJ if successful, 0 on error
*
* A and C are mxm
*
*/
int LEVMAR_COVAR(LM_REAL *JtJ, LM_REAL *C, LM_REAL sumsq, int m, int n)
{
register int i;
int rnk;
LM_REAL fact;
#ifdef HAVE_LAPACK
rnk=LEVMAR_PSEUDOINVERSE(JtJ, C, m);
if(!rnk) return 0;
#else
#ifdef _MSC_VER
#pragma message("LAPACK not available, LU will be used for matrix inversion when computing the covariance; this might be unstable at times")
#else
#warning LAPACK not available, LU will be used for matrix inversion when computing the covariance; this might be unstable at times
#endif // _MSC_VER
rnk=LEVMAR_LUINVERSE(JtJ, C, m);
if(!rnk) return 0;
rnk=m; /* assume full rank */
#endif /* HAVE_LAPACK */
fact=sumsq/(LM_REAL)(n-rnk);
for(i=0; i<m*m; ++i)
C[i]*=fact;
return rnk;
}
/* standard deviation of the best-fit parameter i.
* covar is the mxm covariance matrix of the best-fit parameters (see also LEVMAR_COVAR()).
*
* The standard deviation is computed as \sigma_{i} = \sqrt{C_{ii}}
*/
LM_REAL LEVMAR_STDDEV(LM_REAL *covar, int m, int i)
{
return (LM_REAL)sqrt(covar[i*m+i]);
}
/* Pearson's correlation coefficient of the best-fit parameters i and j.
* covar is the mxm covariance matrix of the best-fit parameters (see also LEVMAR_COVAR()).
*
* The coefficient is computed as \rho_{ij} = C_{ij} / sqrt(C_{ii} C_{jj})
*/
LM_REAL LEVMAR_CORCOEF(LM_REAL *covar, int m, int i, int j)
{
return (LM_REAL)(covar[i*m+j]/sqrt(covar[i*m+i]*covar[j*m+j]));
}
/* check box constraints for consistency */
int LEVMAR_BOX_CHECK(LM_REAL *lb, LM_REAL *ub, int m)
{
register int i;
if(!lb || !ub) return 1;
for(i=0; i<m; ++i)
if(lb[i]>ub[i]) return 0;
return 1;
}
#ifdef HAVE_LAPACK
/* compute the Cholesky decompostion 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 (in column-major order!) so that LAPACK won't destroy it */
for(i=0; i<m; i++)
for(j=0; j<m; j++)
W[i+j*m]=C[i*m+j];
/* cholesky decomposition */
POTF2("U", (int *)&m, W, (int *)&m, (int *)&info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, "LAPACK error: illegal value for argument %d of dpotf2 in %s\n", -info, LCAT(LEVMAR_DER, "()"));
exit(1);
}
else{
fprintf(stderr, "LAPACK error: the leading minor of order %d is not positive definite,\n%s()\n", info,
RCAT("and the cholesky factorization could not be completed in ", LEVMAR_CHOLESKY));
return LM_ERROR;
}
}
/* the decomposition is in the upper part of W (in column-major order!).
* copying it to the lower part and zeroing the upper transposes
* W in row-major order
*/
for(i=0; i<m; i++)
for(j=0; j<i; j++){
W[i+j*m]=W[j+i*m];
W[j+i*m]=0.0;
}
return 0;
}
#endif /* HAVE_LAPACK */
/* Compute e=x-y for two n-vectors x and y and return the squared L2 norm of e.
* e can coincide with either x or y; x can be NULL, in which case it is assumed
* to be equal to the zero vector.
* Uses loop unrolling and blocking to reduce bookkeeping overhead & pipeline
* stalls and increase instruction-level parallelism; see http://www.abarnett.demon.co.uk/tutorial.html
*/
LM_REAL LEVMAR_L2NRMXMY(LM_REAL *e, LM_REAL *x, LM_REAL *y, int n)
{
const int blocksize=8, bpwr=3; /* 8=2^3 */
register int i;
int j1, j2, j3, j4, j5, j6, j7;
int blockn;
register LM_REAL sum0=0.0, sum1=0.0, sum2=0.0, sum3=0.0;
/* n may not be divisible by blocksize,
* go as near as we can first, then tidy up.
*/
blockn = (n>>bpwr)<<bpwr; /* (n / blocksize) * blocksize; */
if(x){
/* unroll the loop in blocks of `blocksize' */
for(i=0; i<blockn; 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)
*/
if(i<n){
/* Jump into the case at the place that will allow
* us to finish off the appropriate number of items.
*/
switch(n - i){
case 7 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
case 6 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
case 5 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
case 4 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
case 3 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
case 2 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
case 1 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
}
}
}
else{ /* x==0 */
/* unroll the loop in blocks of `blocksize' */
for(i=0; i<blockn; 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)
*/
if(i<n){
/* Jump into the case at the place that will allow
* us to finish off the appropriate number of items.
*/
switch(n - i){
case 7 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
case 6 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
case 5 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
case 4 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
case 3 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
case 2 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
case 1 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
}
}
}
return sum0+sum1+sum2+sum3;
}
/* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */
#undef LEVMAR_PSEUDOINVERSE
#undef LEVMAR_LUINVERSE
#undef LEVMAR_BOX_CHECK
#undef LEVMAR_CHOLESKY
#undef LEVMAR_COVAR
#undef LEVMAR_STDDEV
#undef LEVMAR_CORCOEF
#undef LEVMAR_CHKJAC
#undef LEVMAR_FDIF_FORW_JAC_APPROX
#undef LEVMAR_FDIF_CENT_JAC_APPROX
#undef LEVMAR_TRANS_MAT_MAT_MULT
#undef LEVMAR_L2NRMXMY

38
src/external/levmar.cmake vendored
View File

@ -2,24 +2,26 @@
# Copyright 2019, 2020, Visual Computing Lab, ISTI - Italian National Research Council
# SPDX-License-Identifier: BSL-1.0
option(ALLOW_BUNDLED_LEVMAR "Allow use of bundled levmar source" ON)
option(MESHLAB_ALLOW_DOWNLOAD_SOURCE_LEVMAR "Allow download and use of levmar source" ON)
set(LEVMAR_DIR ${CMAKE_CURRENT_LIST_DIR}/levmar-2.3)
if(MESHLAB_ALLOW_DOWNLOAD_SOURCE_LEVMAR)
set(LEVMAR_VER 2.6)
set(LEVMAR_DIR ${MESHLAB_EXTERNAL_DOWNLOAD_DIR}/levmar-${LEVMAR_VER})
if(ALLOW_BUNDLED_LEVMAR AND EXISTS "${LEVMAR_DIR}/lm.h")
message(STATUS "- levmar - using bundled source")
add_library(
external-levmar STATIC
"${LEVMAR_DIR}/compiler.h"
"${LEVMAR_DIR}/lm.h"
"${LEVMAR_DIR}/misc.h"
"${LEVMAR_DIR}/Axb.c"
"${LEVMAR_DIR}/lm.c"
"${LEVMAR_DIR}/lmbc.c"
"${LEVMAR_DIR}/lmblec.c"
"${LEVMAR_DIR}/lmlec.c"
"${LEVMAR_DIR}/misc.c")
target_include_directories(external-levmar PUBLIC ${LEVMAR_DIR})
set_property(TARGET external-levmar PROPERTY FOLDER External)
target_link_libraries(external-levmar PRIVATE external-disable-warnings)
if (NOT EXISTS "${LEVMAR_DIR}/lm.h")
set(LEVMAR_LINK http://users.ics.forth.gr/~lourakis/levmar/levmar-${LEVMAR_VER}.tgz)
download_and_unzip(${LEVMAR_LINK} ${MESHLAB_EXTERNAL_DOWNLOAD_DIR} "Levmar")
endif()
message(STATUS "- levmar - using downloaded source")
set(HAVE_LAPACK 0 CACHE BOOL "Do we have LAPACK/BLAS?")
set(BUILD_DEMO OFF)
set(MESSAGE_QUIET ON)
add_subdirectory(${LEVMAR_DIR})
unset(MESSAGE_QUIET)
add_library(external-levmar INTERFACE)
target_link_libraries(external-levmar INTERFACE levmar)
target_include_directories(external-levmar INTERFACE ${LEVMAR_DIR})
endif()

2
src/meshlabplugins/edit_mutualcorrs/levmarmethods.h
View File

@ -12,7 +12,7 @@ sufficient to get a calibrated shot.<br>
#include <list>
#include "lm.h"
#include "levmar.h"
struct LevmarCorrelation {

2
src/meshlabplugins/edit_mutualcorrs/solver.h
View File

@ -5,7 +5,7 @@
#include "alignset.h"
#include "parameters.h"
#include "lm.h"
#include "levmar.h"
#include <iostream>
#include <fstream>

2
src/meshlabplugins/filter_isoparametrization/param_collapse.h
View File

@ -14,7 +14,7 @@
#include <local_parametrization.h>
#include <mesh_operators.h>
#include <lm.h>
#include <levmar.h>
#include <uv_grid.h>
#include "opt_patch.h"

2
src/meshlabplugins/filter_isoparametrization/parametrizator.h
View File

@ -32,7 +32,7 @@
#include <vcg/space/color4.h>
#include <dual_coord_optimization.h>
#include <float.h>
#include <lm.h>
#include <levmar.h>
#ifndef _MESHLAB
#include <wrap/io_trimesh/export_ply.h>
#endif

2
src/meshlabplugins/filter_mutualglobal/levmarmethods.h
View File

@ -12,7 +12,7 @@ sufficient to get a calibrated shot.<br>
#include <list>
#include "lm.h"
#include "levmar.h"
struct LevmarCorrelation {

2
src/meshlabplugins/filter_mutualglobal/solver.h
View File

@ -5,7 +5,7 @@
#include "alignset.h"
#include "parameters.h"
#include "lm.h"
#include "levmar.h"
#include <iostream>
#include <fstream>

2
src/meshlabplugins/filter_mutualinfo/levmarmethods.h
View File

@ -12,7 +12,7 @@ sufficient to get a calibrated shot.<br>
#include <list>
#include "lm.h"
#include "levmar.h"
struct LevmarCorrelation {

2
src/meshlabplugins/filter_mutualinfo/solver.h
View File

@ -5,7 +5,7 @@
#include "alignset.h"
#include "parameters.h"
#include "lm.h"
#include "levmar.h"
#include <iostream>
#include <fstream>