summaryrefslogtreecommitdiff
path: root/libmoped
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2010-06-25 01:51:45 -0400
committerGuillaume Seguin <guillaume@segu.in>2010-06-25 01:51:45 -0400
commit2855df5c4610c4414258dd53de3e2cd21fde623f (patch)
tree44f2888fc02f32a5b31cbcc7b0463fcd12cf9aaa /libmoped
parent7490195addb30b95a4323076139bd9ac61067a6f (diff)
downloadcmu-2855df5c4610c4414258dd53de3e2cd21fde623f.tar.gz
cmu-2855df5c4610c4414258dd53de3e2cd21fde623f.tar.bz2
Import sba-1.6
Diffstat (limited to 'libmoped')
-rw-r--r--libmoped/libs/sba-1.6/CMakeLists.txt33
-rw-r--r--libmoped/libs/sba-1.6/LICENSE340
-rw-r--r--libmoped/libs/sba-1.6/Makefile34
-rw-r--r--libmoped/libs/sba-1.6/Makefile.icc39
-rw-r--r--libmoped/libs/sba-1.6/Makefile.vc47
-rw-r--r--libmoped/libs/sba-1.6/README.txt65
-rw-r--r--libmoped/libs/sba-1.6/compiler.h42
-rw-r--r--libmoped/libs/sba-1.6/sba.h155
-rw-r--r--libmoped/libs/sba-1.6/sba_chkjac.c458
-rw-r--r--libmoped/libs/sba-1.6/sba_chkjac.h67
-rw-r--r--libmoped/libs/sba-1.6/sba_crsm.c346
-rw-r--r--libmoped/libs/sba-1.6/sba_lapack.c1678
-rw-r--r--libmoped/libs/sba-1.6/sba_levmar.c2528
-rw-r--r--libmoped/libs/sba-1.6/sba_levmar_wrap.c896
-rw-r--r--libmoped/libs/sba-1.6/utils/README.txt37
-rw-r--r--libmoped/libs/sba-1.6/utils/SBAio.pm239
-rw-r--r--libmoped/libs/sba-1.6/utils/hess2eps.c144
-rwxr-xr-xlibmoped/libs/sba-1.6/utils/quats.pl122
-rwxr-xr-xlibmoped/libs/sba-1.6/utils/reprerr.pl523
19 files changed, 7793 insertions, 0 deletions
diff --git a/libmoped/libs/sba-1.6/CMakeLists.txt b/libmoped/libs/sba-1.6/CMakeLists.txt
new file mode 100644
index 0000000..8fb50b9
--- /dev/null
+++ b/libmoped/libs/sba-1.6/CMakeLists.txt
@@ -0,0 +1,33 @@
+# sba CMake file; see http://www.cmake.org and
+# http://www.insightsoftwareconsortium.org/wiki/index.php/CMake_Tutorial
+
+PROJECT(SBA)
+#CMAKE_MINIMUM_REQUIRED(VERSION 1.4)
+
+# 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(.)
+# sba library source files
+ADD_LIBRARY(sba STATIC
+ sba_levmar.c sba_levmar_wrap.c sba_lapack.c sba_crsm.c sba_chkjac.c
+ sba.h sba_chkjac.h compiler.h
+)
+
+#ADD_SUBDIRECTORY(demo)
+SUBDIRS(demo)
diff --git a/libmoped/libs/sba-1.6/LICENSE b/libmoped/libs/sba-1.6/LICENSE
new file mode 100644
index 0000000..d60c31a
--- /dev/null
+++ b/libmoped/libs/sba-1.6/LICENSE
@@ -0,0 +1,340 @@
+ GNU GENERAL PUBLIC LICENSE
+ Version 2, June 1991
+
+ Copyright (C) 1989, 1991 Free Software Foundation, Inc.
+ 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+ Preamble
+
+ The licenses for most software are designed to take away your
+freedom to share and change it. By contrast, the GNU General Public
+License is intended to guarantee your freedom to share and change free
+software--to make sure the software is free for all its users. This
+General Public License applies to most of the Free Software
+Foundation's software and to any other program whose authors commit to
+using it. (Some other Free Software Foundation software is covered by
+the GNU Library General Public License instead.) You can apply it to
+your programs, too.
+
+ When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+this service if you wish), that you receive source code or can get it
+if you want it, that you can change the software or use pieces of it
+in new free programs; and that you know you can do these things.
+
+ To protect your rights, we need to make restrictions that forbid
+anyone to deny you these rights or to ask you to surrender the rights.
+These restrictions translate to certain responsibilities for you if you
+distribute copies of the software, or if you modify it.
+
+ For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must give the recipients all the rights that
+you have. You must make sure that they, too, receive or can get the
+source code. And you must show them these terms so they know their
+rights.
+
+ We protect your rights with two steps: (1) copyright the software, and
+(2) offer you this license which gives you legal permission to copy,
+distribute and/or modify the software.
+
+ Also, for each author's protection and ours, we want to make certain
+that everyone understands that there is no warranty for this free
+software. If the software is modified by someone else and passed on, we
+want its recipients to know that what they have is not the original, so
+that any problems introduced by others will not reflect on the original
+authors' reputations.
+
+ Finally, any free program is threatened constantly by software
+patents. We wish to avoid the danger that redistributors of a free
+program will individually obtain patent licenses, in effect making the
+program proprietary. To prevent this, we have made it clear that any
+patent must be licensed for everyone's free use or not licensed at all.
+
+ The precise terms and conditions for copying, distribution and
+modification follow.
+
+ GNU GENERAL PUBLIC LICENSE
+ TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+ 0. This License applies to any program or other work which contains
+a notice placed by the copyright holder saying it may be distributed
+under the terms of this General Public License. The "Program", below,
+refers to any such program or work, and a "work based on the Program"
+means either the Program or any derivative work under copyright law:
+that is to say, a work containing the Program or a portion of it,
+either verbatim or with modifications and/or translated into another
+language. (Hereinafter, translation is included without limitation in
+the term "modification".) Each licensee is addressed as "you".
+
+Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope. The act of
+running the Program is not restricted, and the output from the Program
+is covered only if its contents constitute a work based on the
+Program (independent of having been made by running the Program).
+Whether that is true depends on what the Program does.
+
+ 1. You may copy and distribute verbatim copies of the Program's
+source code as you receive it, in any medium, provided that you
+conspicuously and appropriately publish on each copy an appropriate
+copyright notice and disclaimer of warranty; keep intact all the
+notices that refer to this License and to the absence of any warranty;
+and give any other recipients of the Program a copy of this License
+along with the Program.
+
+You may charge a fee for the physical act of transferring a copy, and
+you may at your option offer warranty protection in exchange for a fee.
+
+ 2. You may modify your copy or copies of the Program or any portion
+of it, thus forming a work based on the Program, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+ a) You must cause the modified files to carry prominent notices
+ stating that you changed the files and the date of any change.
+
+ b) You must cause any work that you distribute or publish, that in
+ whole or in part contains or is derived from the Program or any
+ part thereof, to be licensed as a whole at no charge to all third
+ parties under the terms of this License.
+
+ c) If the modified program normally reads commands interactively
+ when run, you must cause it, when started running for such
+ interactive use in the most ordinary way, to print or display an
+ announcement including an appropriate copyright notice and a
+ notice that there is no warranty (or else, saying that you provide
+ a warranty) and that users may redistribute the program under
+ these conditions, and telling the user how to view a copy of this
+ License. (Exception: if the Program itself is interactive but
+ does not normally print such an announcement, your work based on
+ the Program is not required to print an announcement.)
+
+These requirements apply to the modified work as a whole. If
+identifiable sections of that work are not derived from the Program,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works. But when you
+distribute the same sections as part of a whole which is a work based
+on the Program, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Program.
+
+In addition, mere aggregation of another work not based on the Program
+with the Program (or with a work based on the Program) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+ 3. You may copy and distribute the Program (or a work based on it,
+under Section 2) in object code or executable form under the terms of
+Sections 1 and 2 above provided that you also do one of the following:
+
+ a) Accompany it with the complete corresponding machine-readable
+ source code, which must be distributed under the terms of Sections
+ 1 and 2 above on a medium customarily used for software interchange; or,
+
+ b) Accompany it with a written offer, valid for at least three
+ years, to give any third party, for a charge no more than your
+ cost of physically performing source distribution, a complete
+ machine-readable copy of the corresponding source code, to be
+ distributed under the terms of Sections 1 and 2 above on a medium
+ customarily used for software interchange; or,
+
+ c) Accompany it with the information you received as to the offer
+ to distribute corresponding source code. (This alternative is
+ allowed only for noncommercial distribution and only if you
+ received the program in object code or executable form with such
+ an offer, in accord with Subsection b above.)
+
+The source code for a work means the preferred form of the work for
+making modifications to it. For an executable work, complete source
+code means all the source code for all modules it contains, plus any
+associated interface definition files, plus the scripts used to
+control compilation and installation of the executable. However, as a
+special exception, the source code distributed need not include
+anything that is normally distributed (in either source or binary
+form) with the major components (compiler, kernel, and so on) of the
+operating system on which the executable runs, unless that component
+itself accompanies the executable.
+
+If distribution of executable or object code is made by offering
+access to copy from a designated place, then offering equivalent
+access to copy the source code from the same place counts as
+distribution of the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+ 4. You may not copy, modify, sublicense, or distribute the Program
+except as expressly provided under this License. Any attempt
+otherwise to copy, modify, sublicense or distribute the Program is
+void, and will automatically terminate your rights under this License.
+However, parties who have received copies, or rights, from you under
+this License will not have their licenses terminated so long as such
+parties remain in full compliance.
+
+ 5. You are not required to accept this License, since you have not
+signed it. However, nothing else grants you permission to modify or
+distribute the Program or its derivative works. These actions are
+prohibited by law if you do not accept this License. Therefore, by
+modifying or distributing the Program (or any work based on the
+Program), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Program or works based on it.
+
+ 6. Each time you redistribute the Program (or any work based on the
+Program), the recipient automatically receives a license from the
+original licensor to copy, distribute or modify the Program subject to
+these terms and conditions. You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties to
+this License.
+
+ 7. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Program at all. For example, if a patent
+license would not permit royalty-free redistribution of the Program by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Program.
+
+If any portion of this section is held invalid or unenforceable under
+any particular circumstance, the balance of the section is intended to
+apply and the section as a whole is intended to apply in other
+circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system, which is
+implemented by public license practices. Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+ 8. If the distribution and/or use of the Program is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Program under this License
+may add an explicit geographical distribution limitation excluding
+those countries, so that distribution is permitted only in or among
+countries not thus excluded. In such case, this License incorporates
+the limitation as if written in the body of this License.
+
+ 9. The Free Software Foundation may publish revised and/or new versions
+of the General Public License from time to time. Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+Each version is given a distinguishing version number. If the Program
+specifies a version number of this License which applies to it and "any
+later version", you have the option of following the terms and conditions
+either of that version or of any later version published by the Free
+Software Foundation. If the Program does not specify a version number of
+this License, you may choose any version ever published by the Free Software
+Foundation.
+
+ 10. If you wish to incorporate parts of the Program into other free
+programs whose distribution conditions are different, write to the author
+to ask for permission. For software which is copyrighted by the Free
+Software Foundation, write to the Free Software Foundation; we sometimes
+make exceptions for this. Our decision will be guided by the two goals
+of preserving the free status of all derivatives of our free software and
+of promoting the sharing and reuse of software generally.
+
+ NO WARRANTY
+
+ 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
+FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
+OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
+PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
+OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
+TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
+PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
+REPAIR OR CORRECTION.
+
+ 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
+REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
+INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
+OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
+TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
+YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
+PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGES.
+
+ END OF TERMS AND CONDITIONS
+
+ How to Apply These Terms to Your New Programs
+
+ If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+ To do so, attach the following notices to the program. It is safest
+to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+ <one line to give the program's name and a brief idea of what it does.>
+ Copyright (C) <year> <name of author>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+
+
+Also add information on how to contact you by electronic and paper mail.
+
+If the program is interactive, make it output a short notice like this
+when it starts in an interactive mode:
+
+ Gnomovision version 69, Copyright (C) year name of author
+ Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+ This is free software, and you are welcome to redistribute it
+ under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License. Of course, the commands you use may
+be called something other than `show w' and `show c'; they could even be
+mouse-clicks or menu items--whatever suits your program.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the program, if
+necessary. Here is a sample; alter the names:
+
+ Yoyodyne, Inc., hereby disclaims all copyright interest in the program
+ `Gnomovision' (which makes passes at compilers) written by James Hacker.
+
+ <signature of Ty Coon>, 1 April 1989
+ Ty Coon, President of Vice
+
+This General Public License does not permit incorporating your program into
+proprietary programs. If your program is a subroutine library, you may
+consider it more useful to permit linking proprietary applications with the
+library. If this is what you want to do, use the GNU Library General
+Public License instead of this License.
diff --git a/libmoped/libs/sba-1.6/Makefile b/libmoped/libs/sba-1.6/Makefile
new file mode 100644
index 0000000..cc6f5d8
--- /dev/null
+++ b/libmoped/libs/sba-1.6/Makefile
@@ -0,0 +1,34 @@
+#
+# Makefile for Sparse Bundle Adjustment library & demo program
+#
+CC=gcc
+#ARCHFLAGS=-march=pentium4 # YOU MIGHT WANT TO UNCOMMENT THIS FOR P4
+CFLAGS=$(ARCHFLAGS) -O3 -funroll-loops -Wall #-Wstrict-aliasing #-g -pg
+OBJS=sba_levmar.o sba_levmar_wrap.o sba_lapack.o sba_crsm.o sba_chkjac.o
+SRCS=sba_levmar.c sba_levmar_wrap.c sba_lapack.c sba_crsm.c sba_chkjac.c
+AR=ar
+RANLIB=ranlib
+MAKE=make
+
+all: libsba.a
+
+libsba.a: $(OBJS)
+ $(AR) crv libsba.a $(OBJS)
+ $(RANLIB) libsba.a
+
+sba_levmar.o: sba.h sba_chkjac.h compiler.h
+sba_levmar_wrap.o: sba.h
+sba_lapack.o: sba.h compiler.h
+sba_crsm.o: sba.h
+sba_chkjac.o: sba.h sba_chkjac.h compiler.h
+
+clean:
+ @rm -f $(OBJS)
+
+realclean cleanall: clean
+ @rm -f libsba.a
+
+depend:
+ makedepend -f Makefile $(SRCS)
+
+# DO NOT DELETE THIS LINE -- make depend depends on it.
diff --git a/libmoped/libs/sba-1.6/Makefile.icc b/libmoped/libs/sba-1.6/Makefile.icc
new file mode 100644
index 0000000..8cecbd9
--- /dev/null
+++ b/libmoped/libs/sba-1.6/Makefile.icc
@@ -0,0 +1,39 @@
+#
+# Makefile for Sparse Bundle Adjustment library & demo program
+#
+CC=icc #-w1 # warnings on
+CXX=icpc
+CFLAGS=-Wcheck -O3 -tpp7 -xW -march=pentium4 -mcpu=pentium4 -ip -ipo -unroll #-g # -fno-alias
+OBJS=sba_levmar.o sba_levmar_wrap.o sba_lapack.o sba_crsm.o sba_chkjac.o
+SRCS=sba_levmar.c sba_levmar_wrap.c sba_lapack.c sba_crsm.c sba_chkjac.c
+AR=xiar
+#RANLIB=ranlib
+MAKE=make
+
+all: libsba.a dem
+
+libsba.a: $(OBJS)
+ $(AR) crvs libsba.a $(OBJS)
+ #$(RANLIB) libsba.a
+
+sba_levmar.o: sba.h sba_chkjac.h compiler.h
+sba_levmar_wrap.o: sba.h
+sba_lapack.o: sba.h compiler.h
+sba_crsm.o: sba.h
+sba_chkjac.o: sba.h sba_chkjac.h compiler.h
+
+dem:
+ cd demo; $(MAKE) -f Makefile.icc
+
+clean:
+ @rm -f $(OBJS)
+ cd demo; $(MAKE) -f Makefile.icc clean
+ cd matlab; $(MAKE) -f Makefile clean
+
+realclean cleanall: clean
+ @rm -f libsba.a
+
+depend:
+ makedepend -f Makefile.icc $(SRCS)
+
+# DO NOT DELETE THIS LINE -- make depend depends on it.
diff --git a/libmoped/libs/sba-1.6/Makefile.vc b/libmoped/libs/sba-1.6/Makefile.vc
new file mode 100644
index 0000000..40b2a37
--- /dev/null
+++ b/libmoped/libs/sba-1.6/Makefile.vc
@@ -0,0 +1,47 @@
+#
+# MS Visual C Makefile for Sparse Bundle Adjustment library & demo program
+# 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/
+#
+CC=cl /nologo
+# 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=/I. /MD /W3 /EHsc /D_CRT_SECURE_NO_DEPRECATE /O2 $(SPOPTFLAGS) # /Wall
+OBJS=sba_levmar.obj sba_levmar_wrap.obj sba_lapack.obj sba_crsm.obj sba_chkjac.obj
+SRCS=sba_levmar.c sba_levmar_wrap.c sba_lapack.c sba_crsm.c sba_chkjac.c
+AR=lib /nologo
+MAKE=nmake /nologo
+
+all: sba.lib dem
+
+sba.lib: $(OBJS)
+ $(AR) /out:sba.lib $(OBJS)
+
+sba_levmar.obj: sba.h sba_chkjac.h compiler.h
+sba_levmar_wrap.obj: sba.h
+sba_lapack.obj: sba.h compiler.h
+sba_crsm.obj: sba.h
+sba_chkjac.obj: sba.h sba_chkjac.h compiler.h
+
+dem:
+ cd demo
+ $(MAKE) /f Makefile.vc
+ cd ..
+
+clean:
+ -del $(OBJS)
+ cd demo
+ $(MAKE) /f Makefile.vc clean
+ cd ..\matlab
+ $(MAKE) /f Makefile.w32 clean
+ cd ..
+
+realclean cleanall: clean
+ -del sba.lib
diff --git a/libmoped/libs/sba-1.6/README.txt b/libmoped/libs/sba-1.6/README.txt
new file mode 100644
index 0000000..6744d79
--- /dev/null
+++ b/libmoped/libs/sba-1.6/README.txt
@@ -0,0 +1,65 @@
+ **************************************************************
+ SBA
+ version 1.6
+ By Manolis Lourakis
+
+ Institute of Computer Science
+ Foundation for Research and Technology - Hellas
+ Heraklion, Crete, Greece
+ **************************************************************
+
+
+==================== GENERAL ====================
+This is sba, a copylefted C/C++ implementation of generic bundle adjustment
+based on the sparse Levenberg-Marquardt algorithm. sba can support a wide
+range of manifestations/parameterizations of the multiple view reconstruction
+problem such as arbitrary projective cameras, partially or fully intrinsically
+calibrated cameras, exterior orientation (i.e. pose) estimation from fixed 3D
+points, etc. sba can be downloaded from http://www.ics.forth.gr/~lourakis/sba
+
+sba relies on lapack for solving the augmented normal equations arising in the
+course of the Levenberg-Marquardt algorithm. if you don't already have lapack,
+I suggest getting clapack from http://www.netlib.org/clapack.
+Directory demo contains eucsbademo, a working example of using sba for Euclidean
+bundle adjustment.
+
+More details regarding sba can be found in ICS/FORTH Technical Report No. 340
+entitled "The Design and Implementation of a Generic Sparse Bundle Adjustment
+Software Package Based on the Levenberg-Marquardt Algorithm", by M.I.A. Lourakis
+and A.A. Argyros (available from http://www.ics.forth.gr/~lourakis/sba)
+
+In case that you use sba in your published work, please include a reference to
+the above TR:
+
+@techreport{lourakis04,
+ author={M.I.A. Lourakis and A.A. Argyros},
+ title={The Design and Implementation of a Generic Sparse Bundle Adjustment Software Package
+ Based on the Levenberg-Marquardt Algorithm}
+ institution={Institute of Computer Science - FORTH},
+ address={Heraklion, Crete, Greece},
+ number={340},
+ year={2004},
+ month={Aug.},
+ note={Available from \verb+http://www.ics.forth.gr/~lourakis/sba+}
+}
+
+==================== FILES ====================
+sba_levmar.c: SBA expert driver routines
+sba_levmar_wrap.c: simple wrappers around the routines in sba_levmar.c
+sba_lapack.c: LAPACK-based linear system solvers (LU, QR, SVD, Cholesky, Bunch-Kaufman)
+sba_crsm.c: CRS sparse matrix manipulation routines
+sba_chkjac.c: routines for verifying the correctness of user-supplied jacobians
+sba.h: Function prototypes & related data structures
+demo/*: Euclidean BA demo; see demo/README.txt for more details
+matlab/*: sba MEX-file interface; see matlab/README.txt for more details
+utils/*: Various utilities; see utils/README.txt for more details
+
+==================== COMPILING ====================
+ - On a Linux/Unix system, typing "make" will build both sba and the demo program.
+
+ - Under Windows and if Visual C is installed & configured for command line use,
+ type "nmake /f Makefile.vc" in a cmd window to build sba and the demo program.
+ In case of trouble, read the comments on top of Makefile.vc
+
+
+Send your comments/bug reports to lourakis@ics.forth.gr
diff --git a/libmoped/libs/sba-1.6/compiler.h b/libmoped/libs/sba-1.6/compiler.h
new file mode 100644
index 0000000..dc0f769
--- /dev/null
+++ b/libmoped/libs/sba-1.6/compiler.h
@@ -0,0 +1,42 @@
+/////////////////////////////////////////////////////////////////////////////////
+////
+//// Compiler-specific definitions for sparse bundle adjustment based on the
+//// Levenberg - Marquardt minimization algorithm
+//// Copyright (C) 2004-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.
+////
+///////////////////////////////////////////////////////////////////////////////////
+
+#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 SBA_FINITE _finite // MSVC
+#elif defined(__ICC) || defined(__INTEL_COMPILER) || defined(__GNUC__)
+#define SBA_FINITE finite // ICC, GCC
+#else
+#define SBA_FINITE finite // other than MSVC, ICC, GCC, let's hope this will work
+#endif
+
+#endif /* _COMPILER_H_ */
diff --git a/libmoped/libs/sba-1.6/sba.h b/libmoped/libs/sba-1.6/sba.h
new file mode 100644
index 0000000..91b7c55
--- /dev/null
+++ b/libmoped/libs/sba-1.6/sba.h
@@ -0,0 +1,155 @@
+/*
+/////////////////////////////////////////////////////////////////////////////////
+////
+//// Prototypes and definitions for sparse bundle adjustment
+//// Copyright (C) 2004-2009 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 _SBA_H_
+#define _SBA_H_
+
+/**************************** Start of configuration options ****************************/
+
+/* define the following if a trailing underscore should be appended to lapack functions names */
+#define SBA_APPEND_UNDERSCORE_SUFFIX // undef this for AIX
+
+
+/* define this to make linear solver routines use the minimum amount of memory
+ * possible. This lowers the memory requirements of large BA problems but will
+ * most likely induce a penalty on performance
+ */
+/*#define SBA_LS_SCARCE_MEMORY */
+
+
+/* define this to save some memory by storing the weight matrices for the image
+ * projections (i.e. wght in the expert drivers) into the memory containing their
+ * covariances (i.e. covx arg). Note that this overwrites covx, making changes
+ * noticeable by the caller
+ */
+/*#define SBA_DESTROY_COVS */
+
+
+/********* End of configuration options, no changes necessary beyond this point *********/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define SBA_MIN_DELTA 1E-06 // finite differentiation minimum delta
+#define SBA_DELTA_SCALE 1E-04 // finite differentiation delta scale
+
+#define SBA_OPTSSZ 5
+#define SBA_INFOSZ 10
+#define SBA_ERROR -1
+#define SBA_INIT_MU 1E-03
+#define SBA_STOP_THRESH 1E-12
+#define SBA_CG_NOPREC 0
+#define SBA_CG_JACOBI 1
+#define SBA_CG_SSOR 2
+#define SBA_VERSION "1.6 (Aug. 2009)"
+
+
+/* Sparse matrix representation using Compressed Row Storage (CRS) format.
+ * See http://www.netlib.org/linalg/html_templates/node91.html#SECTION00931100000000000000
+ */
+
+struct sba_crsm{
+ int nr, nc; /* #rows, #cols for the sparse matrix */
+ int nnz; /* number of nonzero array elements */
+ int *val; /* storage for nonzero array elements. size: nnz */
+ int *colidx; /* column indexes of nonzero elements. size: nnz */
+ int *rowptr; /* locations in val that start a row. size: nr+1.
+ * By convention, rowptr[nr]=nnz
+ */
+};
+
+/* sparse LM */
+
+/* simple drivers */
+extern int
+sba_motstr_levmar(const int n, const int ncon, const int m, const int mcon, char *vmask,
+ double *p, const int cnp, const int pnp, double *x, double *covx, const int mnp,
+ void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata),
+ void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata),
+ void *adata, const int itmax, const int verbose, const double opts[SBA_OPTSSZ], double info[SBA_INFOSZ]);
+
+extern int
+sba_mot_levmar(const int n, const int m, const int mcon, char *vmask, double *p, const int cnp,
+ double *x, double *covx, const int mnp,
+ void (*proj)(int j, int i, double *aj, double *xij, void *adata),
+ void (*projac)(int j, int i, double *aj, double *Aij, void *adata),
+ void *adata, const int itmax, const int verbose, const double opts[SBA_OPTSSZ], double info[SBA_INFOSZ]);
+
+extern int
+sba_str_levmar(const int n, const int ncon, const int m, char *vmask, double *p, const int pnp,
+ double *x, double *covx, const int mnp,
+ void (*proj)(int j, int i, double *bi, double *xij, void *adata),
+ void (*projac)(int j, int i, double *bi, double *Bij, void *adata),
+ void *adata, const int itmax, const int verbose, const double opts[SBA_OPTSSZ], double info[SBA_INFOSZ]);
+
+
+/* expert drivers */
+extern int
+sba_motstr_levmar_x(const int n, const int ncon, const int m, const int mcon, char *vmask, double *p,
+ const int cnp, const int pnp, double *x, double *covx, const int mnp,
+ void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
+ void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
+ void *adata, const int itmax, const int verbose, const double opts[SBA_OPTSSZ], double info[SBA_INFOSZ]);
+
+extern int
+sba_mot_levmar_x(const int n, const int m, const int mcon, char *vmask, double *p, const int cnp,
+ double *x, double *covx, const int mnp,
+ void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
+ void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
+ void *adata, const int itmax, const int verbose, const double opts[SBA_OPTSSZ], double info[SBA_INFOSZ]);
+
+extern int
+sba_str_levmar_x(const int n, const int ncon, const int m, char *vmask, double *p, const int pnp,
+ double *x, double *covx, const int mnp,
+ void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
+ void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
+ void *adata, const int itmax, const int verbose, const double opts[SBA_OPTSSZ], double info[SBA_INFOSZ]);
+
+
+/* interfaces to LAPACK routines: solution of linear systems, matrix inversion, cholesky of inverse */
+extern int sba_Axb_QR(double *A, double *B, double *x, int m, int iscolmaj);
+extern int sba_Axb_QRnoQ(double *A, double *B, double *x, int m, int iscolmaj);
+extern int sba_Axb_Chol(double *A, double *B, double *x, int m, int iscolmaj);
+extern int sba_Axb_LDLt(double *A, double *B, double *x, int m, int iscolmaj);
+extern int sba_Axb_LU(double *A, double *B, double *x, int m, int iscolmaj);
+extern int sba_Axb_SVD(double *A, double *B, double *x, int m, int iscolmaj);
+extern int sba_Axb_BK(double *A, double *B, double *x, int m, int iscolmaj);
+extern int sba_Axb_CG(double *A, double *B, double *x, int m, int niter, double eps, int prec, int iscolmaj);
+extern int sba_symat_invert_LU(double *A, int m);
+extern int sba_symat_invert_Chol(double *A, int m);
+extern int sba_symat_invert_BK(double *A, int m);
+extern int sba_mat_cholinv(double *A, double *B, int m);
+
+/* CRS sparse matrices manipulation routines */
+extern void sba_crsm_alloc(struct sba_crsm *sm, int nr, int nc, int nnz);
+extern void sba_crsm_free(struct sba_crsm *sm);
+extern int sba_crsm_elmidx(struct sba_crsm *sm, int i, int j);
+extern int sba_crsm_elmidxp(struct sba_crsm *sm, int i, int j, int jp, int jpidx);
+extern int sba_crsm_row_elmidxs(struct sba_crsm *sm, int i, int *vidxs, int *jidxs);
+extern int sba_crsm_col_elmidxs(struct sba_crsm *sm, int j, int *vidxs, int *iidxs);
+/* extern int sba_crsm_common_row(struct sba_crsm *sm, int j, int k); */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _SBA_H_ */
diff --git a/libmoped/libs/sba-1.6/sba_chkjac.c b/libmoped/libs/sba-1.6/sba_chkjac.c
new file mode 100644
index 0000000..95ea18f
--- /dev/null
+++ b/libmoped/libs/sba-1.6/sba_chkjac.c
@@ -0,0 +1,458 @@
+/////////////////////////////////////////////////////////////////////////////////
+////
+//// Verification routines for the jacobians employed in the expert & simple drivers
+//// for sparse bundle adjustment based on the Levenberg - Marquardt minimization algorithm
+//// Copyright (C) 2005-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 <float.h>
+
+#include "compiler.h"
+#include "sba.h"
+
+#define emalloc(sz) emalloc_(__FILE__, __LINE__, sz)
+
+#define FABS(x) (((x)>=0)? (x) : -(x))
+
+
+/* auxiliary memory allocation routine with error checking */
+inline static void *emalloc_(char *file, int line, size_t sz)
+{
+void *ptr;
+
+ ptr=(void *)malloc(sz);
+ if(ptr==NULL){
+ fprintf(stderr, "SBA: memory allocation request for %u bytes failed in file %s, line %d, exiting", sz, file, line);
+ exit(1);
+ }
+
+ return ptr;
+}
+
+/*
+ * Check the jacobian of a projection function in nvars variables
+ * evaluated at a point p, for consistency with the function itself.
+ * Expert version
+ *
+ * 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^{nvars} --> R^{nobs}: Given a p in R^{nvars}
+ * it yields hx in R^{nobs}
+ * jacf points to a function implementing the jacobian of func, whose consistency with
+ * func is to be tested. Given a p in R^{nvars}, jacf computes into the nvis*(Asz+Bsz)
+ * matrix jac the jacobian of func at p. Note the jacobian is sparse, consisting of
+ * all A_ij, B_ij and that row i of jac corresponds to the gradient of the i-th
+ * component of func, evaluated at p.
+ * p is an input array of length nvars containing the point of evaluation.
+ * idxij, rcidxs, rcsubs, ncon, mcon, cnp, pnp, mnp are as usual. Note that if cnp=0 or
+ * pnp=0 a jacobian corresponding resp. to motion or camera parameters
+ * only is assumed.
+ * func_adata, jac_adata point to possible additional data and are passed
+ * uninterpreted to func, jacf respectively.
+ * err is an array of length nobs. 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.
+ *
+ * CAUTION: THIS FUNCTION IS NOT 100% FOOLPROOF. The
+ * following excerpt comes from CHKDER's documentation:
+ *
+ * "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 sba_motstr_chkjac_x(
+ void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
+ void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
+ double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int ncon, int mcon, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
+{
+const double factor=100.0, one=1.0, zero=0.0;
+double *fvec, *fjac, *pp, *fvecp, *buf, *err;
+
+int nvars, nobs, m, n, Asz, Bsz, ABsz, nnz;
+register int i, j, ii, jj;
+double eps, epsf, temp, epsmch, epslog;
+register double *ptr1, *ptr2, *pab;
+double *pa, *pb;
+int fvec_sz, pp_sz, fvecp_sz, numerr=0;
+
+ nobs=idxij->nnz*mnp;
+ n=idxij->nr; m=idxij->nc;
+ nvars=m*cnp + n*pnp;
+ epsmch=DBL_EPSILON;
+ eps=sqrt(epsmch);
+
+ Asz=mnp*cnp; Bsz=mnp*pnp; ABsz=Asz+Bsz;
+ fjac=(double *)emalloc(idxij->nnz*ABsz*sizeof(double));
+
+ fvec_sz=fvecp_sz=nobs;
+ pp_sz=nvars;
+ buf=(double *)emalloc((fvec_sz + pp_sz + fvecp_sz)*sizeof(double));
+ fvec=buf;
+ pp=fvec+fvec_sz;
+ fvecp=pp+pp_sz;
+
+ err=(double *)emalloc(nobs*sizeof(double));
+
+ /* compute fvec=func(p) */
+ (*func)(p, idxij, rcidxs, rcsubs, fvec, func_adata);
+
+ /* compute the jacobian at p */
+ (*jacf)(p, idxij, rcidxs, rcsubs, fjac, jac_adata);
+
+ /* compute pp */
+ for(j=0; j<nvars; ++j){
+ temp=eps*FABS(p[j]);
+ if(temp==zero) temp=eps;
+ pp[j]=p[j]+temp;
+ }
+
+ /* compute fvecp=func(pp) */
+ (*func)(pp, idxij, rcidxs, rcsubs, fvecp, func_adata);
+
+ epsf=factor*epsmch;
+ epslog=log10(eps);
+
+ for(i=0; i<nobs; ++i)
+ err[i]=zero;
+
+ pa=p;
+ pb=p + m*cnp;
+ for(i=0; i<n; ++i){
+ nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero A_ij, B_ij, j=0...m-1, actual column numbers in rcsubs */
+ for(j=0; j<nnz; ++j){
+ ptr2=err + idxij->val[rcidxs[j]]*mnp; // set ptr2 to point into err
+
+ if(cnp && rcsubs[j]>=mcon){ // A_ij is nonzero
+ ptr1=fjac + idxij->val[rcidxs[j]]*ABsz; // set ptr1 to point to A_ij
+ pab=pa + rcsubs[j]*cnp;
+ for(jj=0; jj<cnp; ++jj){
+ temp=FABS(pab[jj]);
+ if(temp==zero) temp=one;
+
+ for(ii=0; ii<mnp; ++ii)
+ ptr2[ii]+=temp*ptr1[ii*cnp+jj];
+ }
+ }
+
+ if(pnp && i>=ncon){ // B_ij is nonzero
+ ptr1=fjac + idxij->val[rcidxs[j]]*ABsz + Asz; // set ptr1 to point to B_ij
+ pab=pb + i*pnp;
+ for(jj=0; jj<pnp; ++jj){
+ temp=FABS(pab[jj]);
+ if(temp==zero) temp=one;
+
+ for(ii=0; ii<mnp; ++ii)
+ ptr2[ii]+=temp*ptr1[ii*pnp+jj];
+ }
+ }
+ }
+ }
+
+ for(i=0; i<nobs; ++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]=(log10(temp) - epslog)/epslog;
+ if(temp>=eps) err[i]=zero;
+ }
+
+ free(fjac);
+ free(buf);
+
+ for(i=0; i<n; ++i){
+ nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero err_ij, j=0...m-1 */
+ for(j=0; j<nnz; ++j){
+ if(i<ncon && rcsubs[j]<mcon) continue; // corresponding gradients are taken to be zero
+
+ ptr1=err + idxij->val[rcidxs[j]]*mnp; // set ptr1 to point into err
+ for(ii=0; ii<mnp; ++ii)
+ if(ptr1[ii]<=0.5){
+ fprintf(stderr, "SBA: gradient %d (corresponding to element %d of the projection of point %d on camera %d) is %s (err=%g)\n",
+ idxij->val[rcidxs[j]]*mnp+ii, ii, i, rcsubs[j], (ptr1[ii]==0.0)? "wrong" : "probably wrong", ptr1[ii]);
+ ++numerr;
+ }
+ }
+ }
+ if(numerr) fprintf(stderr, "SBA: found %d suspicious gradients out of %d\n\n", numerr, nobs);
+
+ free(err);
+
+ return;
+}
+
+void sba_mot_chkjac_x(
+ void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
+ void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
+ double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int mcon, int cnp, int mnp, void *func_adata, void *jac_adata)
+{
+ sba_motstr_chkjac_x(func, jacf, p, idxij, rcidxs, rcsubs, 0, mcon, cnp, 0, mnp, func_adata, jac_adata);
+}
+
+void sba_str_chkjac_x(
+ void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
+ void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
+ double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int ncon, int pnp, int mnp, void *func_adata, void *jac_adata)
+{
+ sba_motstr_chkjac_x(func, jacf, p, idxij, rcidxs, rcsubs, ncon, 0, 0, pnp, mnp, func_adata, jac_adata);
+}
+
+#if 0
+/* Routines for directly checking the jacobians supplied to the simple drivers.
+ * They shouldn't be necessary since these jacobians can be verified indirectly
+ * through the expert sba_XXX_chkjac_x() routines.
+ */
+
+/*****************************************************************************************/
+// Sample code for using sba_motstr_chkjac():
+
+ for(i=ncon; i<n; ++i)
+ for(j=mcon; j<m; ++j){
+ if(!vmask[i*m+j]) continue; // point i does not appear in image j
+
+ sba_motstr_chkjac(proj, projac, p+j*cnp, p+m*cnp+i*pnp, j, i, cnp, pnp, mnp, adata, adata);
+ }
+
+
+/*****************************************************************************************/
+
+
+/* union used for passing pointers to the user-supplied functions for the motstr/mot/str simple drivers */
+union proj_projac{
+ struct{
+ void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata);
+ void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata);
+ } motstr;
+
+ struct{
+ void (*proj)(int j, int i, double *aj, double *xij, void *adata);
+ void (*projac)(int j, int i, double *aj, double *Aij, void *adata);
+ } mot;
+
+ struct{
+ void (*proj)(int j, int i, double *bi, double *xij, void *adata);
+ void (*projac)(int j, int i, double *bi, double *Bij, void *adata);
+ } str;
+};
+
+
+/*
+ * Check the jacobian of a projection function in cnp+pnp variables
+ * evaluated at a point p, for consistency with the function itself.
+ * Simple version of the above, NOT to be called directly
+ *
+ * Based on fortran77 subroutine CHKDER by
+ * Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
+ * Argonne National Laboratory. MINPACK project. March 1980.
+ *
+ *
+ * proj points to a function from R^{cnp+pnp} --> R^{mnp}: Given a p=(aj, bi) in R^{cnp+pnp}
+ * it yields hx in R^{mnp}
+ * projac points to a function implementing the jacobian of func, whose consistency with proj
+ * is to be tested. Given a p in R^{cnp+pnp}, jacf computes into the matrix jac=[Aij | Bij]
+ * jacobian of proj at p. Note that row i of jac corresponds to the gradient of the i-th
+ * component of proj, evaluated at p.
+ * aj, bi are input arrays of lengths cnp, pnp containing the parameters for the point of
+ * evaluation, i.e. j-th camera and i-th point
+ * jj, ii specify the point (ii) whose projection jacobian in image (jj) is being checked
+ * cnp, pnp, mnp are as usual. Note that if cnp=0 or
+ * pnp=0 a jacobian corresponding resp. to motion or camera parameters
+ * only is assumed.
+ * func_adata, jac_adata point to possible additional data and are passed
+ * uninterpreted to func, jacf respectively.
+ * err is an array of length mnp. 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.
+ *
+ * CAUTION: THIS FUNCTION IS NOT 100% FOOLPROOF. The
+ * following excerpt comes from CHKDER's documentation:
+ *
+ * "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."
+ */
+
+static void sba_chkjac(
+ union proj_projac *funcs, double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
+{
+const double factor=100.0, one=1.0, zero=0.0;
+double *fvec, *fjac, *Aij, *Bij, *ajp, *bip, *fvecp, *buf, *err;
+
+int Asz, Bsz;
+register int i, j;
+double eps, epsf, temp, epsmch, epslog;
+int fvec_sz, ajp_sz, bip_sz, fvecp_sz, err_sz, numerr=0;
+
+ epsmch=DBL_EPSILON;
+ eps=sqrt(epsmch);
+
+ Asz=mnp*cnp; Bsz=mnp*pnp;
+ fjac=(double *)emalloc((Asz+Bsz)*sizeof(double));
+ Aij=fjac;
+ Bij=Aij+Asz;
+
+ fvec_sz=fvecp_sz=mnp;
+ ajp_sz=cnp; bip_sz=pnp;
+ err_sz=mnp;
+ buf=(double *)emalloc((fvec_sz + ajp_sz + bip_sz + fvecp_sz + err_sz)*sizeof(double));
+ fvec=buf;
+ ajp=fvec+fvec_sz;
+ bip=ajp+ajp_sz;
+ fvecp=bip+bip_sz;
+ err=fvecp+fvecp_sz;
+
+ /* compute fvec=proj(p), p=(aj, bi) & the jacobian at p */
+ if(cnp && pnp){
+ (*(funcs->motstr.proj))(jj, ii, aj, bi, fvec, func_adata);
+ (*(funcs->motstr.projac))(jj, ii, aj, bi, Aij, Bij, jac_adata);
+ }
+ else if(cnp){
+ (*(funcs->mot.proj))(jj, ii, aj, fvec, func_adata);
+ (*(funcs->mot.projac))(jj, ii, aj, Aij, jac_adata);
+ }
+ else{
+ (*(funcs->str.proj))(jj, ii, bi, fvec, func_adata);
+ (*(funcs->str.projac))(jj, ii, bi, Bij, jac_adata);
+ }
+
+ /* compute pp, pp=(ajp, bip) */
+ for(j=0; j<cnp; ++j){
+ temp=eps*FABS(aj[j]);
+ if(temp==zero) temp=eps;
+ ajp[j]=aj[j]+temp;
+ }
+ for(j=0; j<pnp; ++j){
+ temp=eps*FABS(bi[j]);
+ if(temp==zero) temp=eps;
+ bip[j]=bi[j]+temp;
+ }
+
+ /* compute fvecp=proj(pp) */
+ if(cnp && pnp)
+ (*(funcs->motstr.proj))(jj, ii, ajp, bip, fvecp, func_adata);
+ else if(cnp)
+ (*(funcs->mot.proj))(jj, ii, ajp, fvecp, func_adata);
+ else
+ (*(funcs->str.proj))(jj, ii, bip, fvecp, func_adata);
+
+ epsf=factor*epsmch;
+ epslog=log10(eps);
+
+ for(i=0; i<mnp; ++i)
+ err[i]=zero;
+
+ for(j=0; j<cnp; ++j){
+ temp=FABS(aj[j]);
+ if(temp==zero) temp=one;
+
+ for(i=0; i<mnp; ++i)
+ err[i]+=temp*Aij[i*cnp+j];
+ }
+ for(j=0; j<pnp; ++j){
+ temp=FABS(bi[j]);
+ if(temp==zero) temp=one;
+
+ for(i=0; i<mnp; ++i)
+ err[i]+=temp*Bij[i*pnp+j];
+ }
+
+ for(i=0; i<mnp; ++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]=(log10(temp) - epslog)/epslog;
+ if(temp>=eps) err[i]=zero;
+ }
+
+ for(i=0; i<mnp; ++i)
+ if(err[i]<=0.5){
+ fprintf(stderr, "SBA: gradient %d (corresponding to element %d of the projection of point %d on camera %d) is %s (err=%g)\n",
+ i, i, ii, jj, (err[i]==0.0)? "wrong" : "probably wrong", err[i]);
+ ++numerr;
+ }
+ if(numerr) fprintf(stderr, "SBA: found %d suspicious gradients out of %d\n\n", numerr, mnp);
+
+ free(fjac);
+ free(buf);
+
+ return;
+}
+
+void sba_motstr_chkjac(
+ void (*proj)(int jj, int ii, double *aj, double *bi, double *xij, void *adata),
+ void (*projac)(int jj, int ii, double *aj, double *bi, double *Aij, double *Bij, void *adata),
+ double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
+{
+union proj_projac funcs;
+
+ funcs.motstr.proj=proj;
+ funcs.motstr.projac=projac;
+
+ sba_chkjac(&funcs, aj, bi, jj, ii, cnp, pnp, mnp, func_adata, jac_adata);
+}
+
+void sba_mot_chkjac(
+ void (*proj)(int jj, int ii, double *aj, double *xij, void *adata),
+ void (*projac)(int jj, int ii, double *aj, double *Aij, void *adata),
+ double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
+{
+union proj_projac funcs;
+
+ funcs.mot.proj=proj;
+ funcs.mot.projac=projac;
+
+ sba_chkjac(&funcs, aj, NULL, jj, ii, cnp, 0, mnp, func_adata, jac_adata);
+}
+
+void sba_str_chkjac(
+ void (*proj)(int jj, int ii, double *bi, double *xij, void *adata),
+ void (*projac)(int jj, int ii, double *bi, double *Bij, void *adata),
+ double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
+{
+union proj_projac funcs;
+
+ funcs.str.proj=proj;
+ funcs.str.projac=projac;
+
+ sba_chkjac(&funcs, NULL, bi, jj, ii, 0, pnp, mnp, func_adata, jac_adata);
+}
+#endif /* 0 */
diff --git a/libmoped/libs/sba-1.6/sba_chkjac.h b/libmoped/libs/sba-1.6/sba_chkjac.h
new file mode 100644
index 0000000..65c9aaa
--- /dev/null
+++ b/libmoped/libs/sba-1.6/sba_chkjac.h
@@ -0,0 +1,67 @@
+/////////////////////////////////////////////////////////////////////////////////
+////
+//// Prototypes and definitions for verification routines for the jacobians
+//// employed in the expert & simple drivers for sparse bundle adjustment
+//// Copyright (C) 2005-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.
+////
+///////////////////////////////////////////////////////////////////////////////////
+
+#ifndef _SBA_CHKJAC_H_
+#define _SBA_CHKJAC_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if 0
+/* simple driver jacobians */
+extern void sba_motstr_chkjac(
+ void (*proj)(int jj, int ii, double *aj, double *bi, double *xij, void *adata),
+ void (*projac)(int jj, int ii, double *aj, double *bi, double *Aij, double *Bij, void *adata),
+ double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata);
+
+extern void sba_mot_chkjac(
+ void (*proj)(int jj, int ii, double *aj, double *xij, void *adata),
+ void (*projac)(int jj, int ii, double *aj, double *Aij, void *adata),
+ double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata);
+
+extern void sba_str_chkjac(
+ void (*proj)(int jj, int ii, double *bi, double *xij, void *adata),
+ void (*projac)(int jj, int ii, double *bi, double *Bij, void *adata),
+ double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata);
+#endif /* 0 */
+
+/* expert driver jacobians */
+extern void sba_motstr_chkjac_x(
+ void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
+ void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
+ double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int ncon, int mcon, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata);
+
+extern void sba_mot_chkjac_x(
+ void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
+ void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
+ double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int mcon, int cnp, int mnp, void *func_adata, void *jac_adata);
+
+extern void sba_str_chkjac_x(
+ void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
+ void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
+ double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int ncon, int pnp, int mnp, void *func_adata, void *jac_adata);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _SBA_CHKJAC_H_ */
diff --git a/libmoped/libs/sba-1.6/sba_crsm.c b/libmoped/libs/sba-1.6/sba_crsm.c
new file mode 100644
index 0000000..22179ea
--- /dev/null
+++ b/libmoped/libs/sba-1.6/sba_crsm.c
@@ -0,0 +1,346 @@
+/////////////////////////////////////////////////////////////////////////////////
+////
+//// CRS sparse matrices manipulation routines
+//// Copyright (C) 2004-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 "sba.h"
+
+static void sba_crsm_print(struct sba_crsm *sm, FILE *fp);
+static void sba_crsm_build(struct sba_crsm *sm, int *m, int nr, int nc);
+
+/* allocate a sparse CRS matrix */
+void sba_crsm_alloc(struct sba_crsm *sm, int nr, int nc, int nnz)
+{
+int msz;
+
+ sm->nr=nr;
+ sm->nc=nc;
+ sm->nnz=nnz;
+ msz=2*nnz+nr+1;
+ sm->val=(int *)malloc(msz*sizeof(int)); /* required memory is allocated in a single step */
+ if(!sm->val){
+ fprintf(stderr, "SBA: memory allocation request failed in sba_crsm_alloc() [nr=%d, nc=%d, nnz=%d]\n", nr, nc, nnz);
+ exit(1);
+ }
+ sm->colidx=sm->val+nnz;
+ sm->rowptr=sm->colidx+nnz;
+}
+
+/* free a sparse CRS matrix */
+void sba_crsm_free(struct sba_crsm *sm)
+{
+ sm->nr=sm->nc=sm->nnz=-1;
+ free(sm->val);
+ sm->val=sm->colidx=sm->rowptr=NULL;
+}
+
+static void sba_crsm_print(struct sba_crsm *sm, FILE *fp)
+{
+register int i;
+
+ fprintf(fp, "matrix is %dx%d, %d non-zeros\nval: ", sm->nr, sm->nc, sm->nnz);
+ for(i=0; i<sm->nnz; ++i)
+ fprintf(fp, "%d ", sm->val[i]);
+ fprintf(fp, "\ncolidx: ");
+ for(i=0; i<sm->nnz; ++i)
+ fprintf(fp, "%d ", sm->colidx[i]);
+ fprintf(fp, "\nrowptr: ");
+ for(i=0; i<=sm->nr; ++i)
+ fprintf(fp, "%d ", sm->rowptr[i]);
+ fprintf(fp, "\n");
+}
+
+/* build a sparse CRS matrix from a dense one. intended to serve as an example for sm creation */
+static void sba_crsm_build(struct sba_crsm *sm, int *m, int nr, int nc)
+{
+int nnz;
+register int i, j, k;
+
+ /* count nonzeros */
+ for(i=nnz=0; i<nr; ++i)
+ for(j=0; j<nc; ++j)
+ if(m[i*nc+j]!=0) ++nnz;
+
+ sba_crsm_alloc(sm, nr, nc, nnz);
+
+ /* fill up the sm structure */
+ for(i=k=0; i<nr; ++i){
+ sm->rowptr[i]=k;
+ for(j=0; j<nc; ++j)
+ if(m[i*nc+j]!=0){
+ sm->val[k]=m[i*nc+j];
+ sm->colidx[k++]=j;
+ }
+ }
+ sm->rowptr[nr]=nnz;
+}
+
+/* returns the index of the (i, j) element. No bounds checking! */
+int sba_crsm_elmidx(struct sba_crsm *sm, int i, int j)
+{
+register int low, high, mid, diff;
+
+ low=sm->rowptr[i];
+ high=sm->rowptr[i+1]-1;
+
+ /* binary search for finding the element at column j */
+ while(low<=high){
+ /* following early termination test seems to actually slow down the search */
+ //if(j<sm->colidx[low] || j>sm->colidx[high]) return -1; /* not found */
+
+ /* mid=low+((high-low)>>1) ensures no index overflows */
+ mid=(low+high)>>1; //(low+high)/2;
+ diff=j-sm->colidx[mid];
+ if(diff<0)
+ high=mid-1;
+ else if(diff>0)
+ low=mid+1;
+ else
+ return mid;
+ }
+
+ return -1; /* not found */
+}
+
+/* similarly to sba_crsm_elmidx() above, returns the index of the (i, j) element using the
+ * fact that the index of element (i, jp) was previously found to be jpidx. This can be
+ * slightly faster than sba_crsm_elmidx(). No bounds checking!
+ */
+int sba_crsm_elmidxp(struct sba_crsm *sm, int i, int j, int jp, int jpidx)
+{
+register int low, high, mid, diff;
+
+ diff=j-jp;
+ if(diff>0){
+ low=jpidx+1;
+ high=sm->rowptr[i+1]-1;
+ }
+ else if(diff==0)
+ return jpidx;
+ else{ /* diff<0 */
+ low=sm->rowptr[i];
+ high=jpidx-1;
+ }
+
+ /* binary search for finding the element at column j */
+ while(low<=high){
+ /* following early termination test seems to actually slow down the search */
+ //if(j<sm->colidx[low] || j>sm->colidx[high]) return -1; /* not found */
+
+ /* mid=low+((high-low)>>1) ensures no index overflows */
+ mid=(low+high)>>1; //(low+high)/2;
+ diff=j-sm->colidx[mid];
+ if(diff<0)
+ high=mid-1;
+ else if(diff>0)
+ low=mid+1;
+ else
+ return mid;
+ }
+
+ return -1; /* not found */
+}
+
+/* returns the number of nonzero elements in row i and
+ * fills up the vidxs and jidxs arrays with the val and column
+ * indexes of the elements found, respectively.
+ * vidxs and jidxs are assumed preallocated and of max. size sm->nc
+ */
+int sba_crsm_row_elmidxs(struct sba_crsm *sm, int i, int *vidxs, int *jidxs)
+{
+register int j, k;
+
+ for(j=sm->rowptr[i], k=0; j<sm->rowptr[i+1]; ++j, ++k){
+ vidxs[k]=j;
+ jidxs[k]=sm->colidx[j];
+ }
+
+ return k;
+}
+
+/* returns the number of nonzero elements in col j and
+ * fills up the vidxs and iidxs arrays with the val and row
+ * indexes of the elements found, respectively.
+ * vidxs and iidxs are assumed preallocated and of max. size sm->nr
+ */
+int sba_crsm_col_elmidxs(struct sba_crsm *sm, int j, int *vidxs, int *iidxs)
+{
+register int *nextrowptr=sm->rowptr+1;
+register int i, l;
+register int low, high, mid, diff;
+
+ for(i=l=0; i<sm->nr; ++i){
+ low=sm->rowptr[i];
+ high=nextrowptr[i]-1;
+
+ /* binary search attempting to find an element at column j */
+ while(low<=high){
+ //if(j<sm->colidx[low] || j>sm->colidx[high]) break; /* not found */
+
+ mid=(low+high)>>1; //(low+high)/2;
+ diff=j-sm->colidx[mid];
+ if(diff<0)
+ high=mid-1;
+ else if(diff>0)
+ low=mid+1;
+ else{ /* found */
+ vidxs[l]=mid;
+ iidxs[l++]=i;
+ break;
+ }
+ }
+ }
+
+ return l;
+}
+
+/* a more straighforward (but slower) implementation of the above function */
+/***
+int sba_crsm_col_elmidxs(struct sba_crsm *sm, int j, int *vidxs, int *iidxs)
+{
+register int i, k, l;
+
+ for(i=l=0; i<sm->nr; ++i)
+ for(k=sm->rowptr[i]; k<sm->rowptr[i+1]; ++k)
+ if(sm->colidx[k]==j){
+ vidxs[l]=k;
+ iidxs[l++]=i;
+ }
+
+ return l;
+}
+***/
+
+#if 0
+/* returns 1 if there exists a row i having columns j and k,
+ * i.e. a row i s.t. elements (i, j) and (i, k) are nonzero;
+ * 0 otherwise
+ */
+int sba_crsm_common_row(struct sba_crsm *sm, int j, int k)
+{
+register int i, low, high, mid, diff;
+
+ if(j==k) return 1;
+
+ for(i=0; i<sm->nr; ++i){
+ low=sm->rowptr[i];
+ high=sm->rowptr[i+1]-1;
+ if(j<sm->colidx[low] || j>sm->colidx[high] || /* j not found */
+ k<sm->colidx[low] || k>sm->colidx[high]) /* k not found */
+ continue;
+
+ /* binary search for finding the element at column j */
+ while(low<=high){
+ mid=(low+high)>>1; //(low+high)/2;
+ diff=j-sm->colidx[mid];
+ if(diff<0)
+ high=mid-1;
+ else if(diff>0)
+ low=mid+1;
+ else
+ goto jfound;
+ }
+
+ continue; /* j not found */
+
+jfound:
+ if(j>k){
+ low=sm->rowptr[i];
+ high=mid-1;
+ }
+ else{
+ low=mid+1;
+ high=sm->rowptr[i+1]-1;
+ }
+
+ if(k<sm->colidx[low] || k>sm->colidx[high]) continue; /* k not found */
+
+ /* binary search for finding the element at column k */
+ while(low<=high){
+ mid=(low+high)>>1; //(low+high)/2;
+ diff=k-sm->colidx[mid];
+ if(diff<0)
+ high=mid-1;
+ else if(diff>0)
+ low=mid+1;
+ else /* found */
+ return 1;
+ }
+ }
+
+ return 0;
+}
+#endif
+
+
+#if 0
+
+/* sample code using the above routines */
+
+main()
+{
+int mat[7][6]={
+ {10, 0, 0, 0, -2, 0},
+ {3, 9, 0, 0, 0, 3},
+ {0, 7, 8, 7, 0, 0},
+ {3, 0, 8, 7, 5, 0},
+ {0, 8, 0, 9, 9, 13},
+ {0, 4, 0, 0, 2, -1},
+ {3, 7, 0, 9, 2, 0}
+};
+
+struct sba_crsm sm;
+int i, j, k, l;
+int vidxs[7], /* max(6, 7) */
+ jidxs[6], iidxs[7];
+
+
+ sba_crsm_build(&sm, mat[0], 7, 6);
+ sba_crsm_print(&sm, stdout);
+
+ for(i=0; i<7; ++i){
+ for(j=0; j<6; ++j)
+ printf("%3d ", ((k=sba_crsm_elmidx(&sm, i, j))!=-1)? sm.val[k] : 0);
+ printf("\n");
+ }
+
+ for(i=0; i<7; ++i){
+ k=sba_crsm_row_elmidxs(&sm, i, vidxs, jidxs);
+ printf("row %d\n", i);
+ for(l=0; l<k; ++l){
+ j=jidxs[l];
+ printf("%d %d ", j, sm.val[vidxs[l]]);
+ }
+ printf("\n");
+ }
+
+ for(j=0; j<6; ++j){
+ k=sba_crsm_col_elmidxs(&sm, j, vidxs, iidxs);
+ printf("col %d\n", j);
+ for(l=0; l<k; ++l){
+ i=iidxs[l];
+ printf("%d %d ", i, sm.val[vidxs[l]]);
+ }
+ printf("\n");
+ }
+
+ sba_crsm_free(&sm);
+}
+#endif
diff --git a/libmoped/libs/sba-1.6/sba_lapack.c b/libmoped/libs/sba-1.6/sba_lapack.c
new file mode 100644
index 0000000..a6779fc
--- /dev/null
+++ b/libmoped/libs/sba-1.6/sba_lapack.c
@@ -0,0 +1,1678 @@
+/////////////////////////////////////////////////////////////////////////////////
+////
+//// Linear algebra operations for the sba package
+//// Copyright (C) 2004-2009 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.
+////
+///////////////////////////////////////////////////////////////////////////////////
+
+/* A note on memory alignment:
+ *
+ * Several of the functions below use a piece of dynamically allocated memory
+ * to store variables of different size (i.e., ints and doubles). To avoid
+ * alignment problems, care must be taken so that elements that are larger
+ * (doubles) are stored before smaller ones (ints). This ensures proper
+ * alignment under different alignment choices made by different CPUs:
+ * For instance, a double variable is aligned on x86 to 4 bytes but
+ * aligned to 8 bytes on AMD64 despite having the same size of 8 bytes.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <float.h>
+
+#include "compiler.h"
+#include "sba.h"
+
+#ifdef SBA_APPEND_UNDERSCORE_SUFFIX
+#define F77_FUNC(func) func ## _
+#else
+#define F77_FUNC(func) func
+#endif /* SBA_APPEND_UNDERSCORE_SUFFIX */
+
+
+/* declarations of LAPACK routines employed */
+
+/* QR decomposition */
+extern int F77_FUNC(dgeqrf)(int *m, int *n, double *a, int *lda, double *tau, double *work, int *lwork, int *info);
+extern int F77_FUNC(dorgqr)(int *m, int *n, int *k, double *a, int *lda, double *tau, double *work, int *lwork, int *info);
+
+/* solution of triangular system */
+extern int F77_FUNC(dtrtrs)(char *uplo, char *trans, char *diag, int *n, int *nrhs, double *a, int *lda, double *b, int *ldb, int *info);
+
+/* Cholesky decomposition, linear system solution and matrix inversion */
+extern int F77_FUNC(dpotf2)(char *uplo, int *n, double *a, int *lda, int *info); /* unblocked Cholesky */
+extern int F77_FUNC(dpotrf)(char *uplo, int *n, double *a, int *lda, int *info); /* block version of dpotf2 */
+extern int F77_FUNC(dpotrs)(char *uplo, int *n, int *nrhs, double *a, int *lda, double *b, int *ldb, int *info);
+extern int F77_FUNC(dpotri)(char *uplo, int *n, double *a, int *lda, int *info);
+
+/* LU decomposition, linear system solution and matrix inversion */
+extern int F77_FUNC(dgetrf)(int *m, int *n, double *a, int *lda, int *ipiv, int *info); /* blocked LU */
+extern int F77_FUNC(dgetf2)(int *m, int *n, double *a, int *lda, int *ipiv, int *info); /* unblocked LU */
+extern int F77_FUNC(dgetrs)(char *trans, int *n, int *nrhs, double *a, int *lda, int *ipiv, double *b, int *ldb, int *info);
+extern int F77_FUNC(dgetri)(int *n, double *a, int *lda, int *ipiv, double *work, int *lwork, int *info);
+
+/* SVD */
+extern int F77_FUNC(dgesvd)(char *jobu, char *jobvt, int *m, int *n,
+ double *a, int *lda, double *s, double *u, int *ldu,
+ double *vt, int *ldvt, double *work, int *lwork,
+ int *info);
+
+/* lapack 3.0 routine, faster than dgesvd() */
+extern int F77_FUNC(dgesdd)(char *jobz, int *m, int *n, double *a, int *lda,
+ double *s, double *u, int *ldu, double *vt, int *ldvt,
+ double *work, int *lwork, int *iwork, int *info);
+
+
+/* Bunch-Kaufman factorization of a real symmetric matrix A, solution of linear systems and matrix inverse */
+extern int F77_FUNC(dsytrf)(char *uplo, int *n, double *a, int *lda, int *ipiv, double *work, int *lwork, int *info); /* blocked ver. */
+extern int F77_FUNC(dsytrs)(char *uplo, int *n, int *nrhs, double *a, int *lda, int *ipiv, double *b, int *ldb, int *info);
+extern int F77_FUNC(dsytri)(char *uplo, int *n, double *a, int *lda, int *ipiv, double *work, int *info);
+
+
+/*
+ * This function returns the solution of Ax = b
+ *
+ * The function is based on QR decomposition with explicit computation of Q:
+ * If A=Q R with Q orthogonal and R upper triangular, the linear system becomes
+ * Q R x = b or R x = Q^T b.
+ *
+ * A is mxm, b is mx1. Argument iscolmaj specifies whether A is
+ * stored in column or row major order. Note that if iscolmaj==1
+ * this function modifies A!
+ *
+ * The function returns 0 in case of error, 1 if successfull
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int sba_Axb_QR(double *A, double *B, double *x, int m, int iscolmaj)
+{
+static double *buf=NULL;
+static int buf_sz=0, nb=0;
+
+double *a, *r, *tau, *work;
+int a_sz, r_sz, tau_sz, tot_sz;
+register int i, j;
+int info, worksz, nrhs=1;
+register double sum;
+
+ if(A==NULL){
+ if(buf) free(buf);
+ buf=NULL;
+ buf_sz=0;
+
+ return 1;
+ }
+
+ /* calculate required memory size */
+ a_sz=(iscolmaj)? 0 : m*m;
+ r_sz=m*m; /* only the upper triangular part really needed */
+ tau_sz=m;
+ if(!nb){
+#ifndef SBA_LS_SCARCE_MEMORY
+ double tmp;
+
+ worksz=-1; // workspace query; optimal size is returned in tmp
+ F77_FUNC(dgeqrf)((int *)&m, (int *)&m, NULL, (int *)&m, NULL, (double *)&tmp, (int *)&worksz, (int *)&info);
+ nb=((int)tmp)/m; // optimal worksize is m*nb
+#else
+ nb=1; // min worksize is m
+#endif /* SBA_LS_SCARCE_MEMORY */
+ }
+ worksz=nb*m;
+ tot_sz=a_sz + r_sz + tau_sz + worksz;
+
+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+ if(buf) free(buf); /* free previously allocated memory */
+
+ buf_sz=tot_sz;
+ buf=(double *)malloc(buf_sz*sizeof(double));
+ if(!buf){
+ fprintf(stderr, "memory allocation in sba_Axb_QR() failed!\n");
+ exit(1);
+ }
+ }
+
+ if(!iscolmaj){
+ a=buf;
+ /* 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];
+ }
+ else a=A; /* no copying required */
+
+ r=buf+a_sz;
+ tau=r+r_sz;
+ work=tau+tau_sz;
+
+ /* QR decomposition of A */
+ F77_FUNC(dgeqrf)((int *)&m, (int *)&m, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
+ /* error treatment */
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "LAPACK error: illegal value for argument %d of dgeqrf in sba_Axb_QR()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "Unknown LAPACK error %d for dgeqrf in sba_Axb_QR()\n", info);
+ return 0;
+ }
+ }
+
+ /* R is now stored in the upper triangular part of a; copy it in r so that dorgqr() below won't destroy it */
+ for(i=0; i<r_sz; ++i)
+ r[i]=a[i];
+
+ /* compute Q using the elementary reflectors computed by the above decomposition */
+ F77_FUNC(dorgqr)((int *)&m, (int *)&m, (int *)&m, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "LAPACK error: illegal value for argument %d of dorgqr in sba_Axb_QR()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "Unknown LAPACK error (%d) in sba_Axb_QR()\n", info);
+ return 0;
+ }
+ }
+
+ /* Q is now in a; compute Q^T b in x */
+ for(i=0; i<m; ++i){
+ for(j=0, sum=0.0; j<m; ++j)
+ sum+=a[i*m+j]*B[j];
+ x[i]=sum;
+ }
+
+ /* solve the linear system R x = Q^t b */
+ F77_FUNC(dtrtrs)("U", "N", "N", (int *)&m, (int *)&nrhs, r, (int *)&m, x, (int *)&m, &info);
+ /* error treatment */
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_QR()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_QR()\n", info);
+ return 0;
+ }
+ }
+
+ return 1;
+}
+
+/*
+ * This function returns the solution of Ax = b
+ *
+ * The function is based on QR decomposition without computation of Q:
+ * If A=Q R with Q orthogonal and R upper triangular, the linear system becomes
+ * (A^T A) x = A^T b or (R^T Q^T Q R) x = A^T b or (R^T R) x = A^T b.
+ * This amounts to solving R^T y = A^T b for y and then R x = y for x
+ * Note that Q does not need to be explicitly computed
+ *
+ * A is mxm, b is mx1. Argument iscolmaj specifies whether A is
+ * stored in column or row major order. Note that if iscolmaj==1
+ * this function modifies A!
+ *
+ * The function returns 0 in case of error, 1 if successfull
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int sba_Axb_QRnoQ(double *A, double *B, double *x, int m, int iscolmaj)
+{
+static double *buf=NULL;
+static int buf_sz=0, nb=0;
+
+double *a, *tau, *work;
+int a_sz, tau_sz, tot_sz;
+register int i, j;
+int info, worksz, nrhs=1;
+register double sum;
+
+ if(A==NULL){
+ if(buf) free(buf);
+ buf=NULL;
+ buf_sz=0;
+
+ return 1;
+ }
+
+ /* calculate required memory size */
+ a_sz=(iscolmaj)? 0 : m*m;
+ tau_sz=m;
+ if(!nb){
+#ifndef SBA_LS_SCARCE_MEMORY
+ double tmp;
+
+ worksz=-1; // workspace query; optimal size is returned in tmp
+ F77_FUNC(dgeqrf)((int *)&m, (int *)&m, NULL, (int *)&m, NULL, (double *)&tmp, (int *)&worksz, (int *)&info);
+ nb=((int)tmp)/m; // optimal worksize is m*nb
+#else
+ nb=1; // min worksize is m
+#endif /* SBA_LS_SCARCE_MEMORY */
+ }
+ worksz=nb*m;
+ tot_sz=a_sz + tau_sz + worksz;
+
+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+ if(buf) free(buf); /* free previously allocated memory */
+
+ buf_sz=tot_sz;
+ buf=(double *)malloc(buf_sz*sizeof(double));
+ if(!buf){
+ fprintf(stderr, "memory allocation in sba_Axb_QRnoQ() failed!\n");
+ exit(1);
+ }
+ }
+
+ if(!iscolmaj){
+ a=buf;
+ /* 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];
+ }
+ else a=A; /* no copying required */
+
+ tau=buf+a_sz;
+ work=tau+tau_sz;
+
+ /* compute A^T b in x */
+ for(i=0; i<m; ++i){
+ for(j=0, sum=0.0; j<m; ++j)
+ sum+=a[i*m+j]*B[j];
+ x[i]=sum;
+ }
+
+ /* QR decomposition of A */
+ F77_FUNC(dgeqrf)((int *)&m, (int *)&m, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
+ /* error treatment */
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "LAPACK error: illegal value for argument %d of dgeqrf in sba_Axb_QRnoQ()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "Unknown LAPACK error %d for dgeqrf in sba_Axb_QRnoQ()\n", info);
+ return 0;
+ }
+ }
+
+ /* R is stored in the upper triangular part of a */
+
+ /* solve the linear system R^T y = A^t b */
+ F77_FUNC(dtrtrs)("U", "T", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
+ /* error treatment */
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_QRnoQ()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_QRnoQ()\n", info);
+ return 0;
+ }
+ }
+
+ /* solve the linear system R x = y */
+ F77_FUNC(dtrtrs)("U", "N", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
+ /* error treatment */
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_QRnoQ()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_QRnoQ()\n", info);
+ return 0;
+ }
+ }
+
+ return 1;
+}
+
+/*
+ * This function returns the solution of Ax=b
+ *
+ * The function assumes that A is symmetric & positive definite and employs
+ * the Cholesky decomposition:
+ * If A=U^T U with U upper triangular, the system to be solved becomes
+ * (U^T U) x = b
+ * This amounts to solving U^T y = b for y and then U x = y for x
+ *
+ * A is mxm, b is mx1. Argument iscolmaj specifies whether A is
+ * stored in column or row major order. Note that if iscolmaj==1
+ * this function modifies A!
+ *
+ * The function returns 0 in case of error, 1 if successfull
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int sba_Axb_Chol(double *A, double *B, double *x, int m, int iscolmaj)
+{
+static double *buf=NULL;
+static int buf_sz=0;
+
+double *a;
+int a_sz, tot_sz;
+register int i, j;
+int info, nrhs=1;
+
+ if(A==NULL){
+ if(buf) free(buf);
+ buf=NULL;
+ buf_sz=0;
+
+ return 1;
+ }
+
+ /* calculate required memory size */
+ a_sz=(iscolmaj)? 0 : m*m;
+ tot_sz=a_sz;
+
+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+ if(buf) free(buf); /* free previously allocated memory */
+
+ buf_sz=tot_sz;
+ buf=(double *)malloc(buf_sz*sizeof(double));
+ if(!buf){
+ fprintf(stderr, "memory allocation in sba_Axb_Chol() failed!\n");
+ exit(1);
+ }
+ }
+
+ if(!iscolmaj){
+ a=buf;
+
+ /* store A into a and B into x; A is assumed to be symmetric, hence
+ * the column and row major order representations are the same
+ */
+ for(i=0; i<m; ++i){
+ a[i]=A[i];
+ x[i]=B[i];
+ }
+ for(j=m*m; i<j; ++i) // copy remaining rows; note that i is not re-initialized
+ a[i]=A[i];
+ }
+ else{ /* no copying is necessary for A */
+ a=A;
+ for(i=0; i<m; ++i)
+ x[i]=B[i];
+ }
+
+ /* Cholesky decomposition of A */
+ //F77_FUNC(dpotf2)("U", (int *)&m, a, (int *)&m, (int *)&info);
+ F77_FUNC(dpotrf)("U", (int *)&m, a, (int *)&m, (int *)&info);
+ /* error treatment */
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "LAPACK error: illegal value for argument %d of dpotf2/dpotrf in sba_Axb_Chol()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "LAPACK error: the leading minor of order %d is not positive definite,\nthe factorization could not be completed for dpotf2/dpotrf in sba_Axb_Chol()\n", info);
+ return 0;
+ }
+ }
+
+ /* below are two alternative ways for solving the linear system: */
+#if 1
+ /* use the computed Cholesky in one lapack call */
+ F77_FUNC(dpotrs)("U", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
+ if(info<0){
+ fprintf(stderr, "LAPACK error: illegal value for argument %d of dpotrs in sba_Axb_Chol()\n", -info);
+ exit(1);
+ }
+#else
+ /* solve the linear systems U^T y = b, U x = y */
+ F77_FUNC(dtrtrs)("U", "T", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
+ /* error treatment */
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_Chol()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_Chol()\n", info);
+ return 0;
+ }
+ }
+
+ /* solve U x = y */
+ F77_FUNC(dtrtrs)("U", "N", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
+ /* error treatment */
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_Chol()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_Chol()\n", info);
+ return 0;
+ }
+ }
+#endif /* 1 */
+
+ return 1;
+}
+
+/*
+ * This function returns the solution of Ax = b
+ *
+ * The function employs LU decomposition:
+ * If A=L U with L lower and U upper triangular, then the original system
+ * amounts to solving
+ * L y = b, U x = y
+ *
+ * A is mxm, b is mx1. Argument iscolmaj specifies whether A is
+ * stored in column or row major order. Note that if iscolmaj==1
+ * this function modifies A!
+ *
+ * The function returns 0 in case of error,
+ * 1 if successfull
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int sba_Axb_LU(double *A, double *B, double *x, int m, int iscolmaj)
+{
+static double *buf=NULL;
+static int buf_sz=0;
+
+int a_sz, ipiv_sz, tot_sz;
+register int i, j;
+int info, *ipiv, nrhs=1;
+double *a;
+
+ if(A==NULL){
+ if(buf) free(buf);
+ buf=NULL;
+ buf_sz=0;
+
+ return 1;
+ }
+
+ /* calculate required memory size */
+ ipiv_sz=m;
+ a_sz=(iscolmaj)? 0 : m*m;
+ tot_sz=a_sz*sizeof(double) + ipiv_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
+
+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+ if(buf) free(buf); /* free previously allocated memory */
+
+ buf_sz=tot_sz;
+ buf=(double *)malloc(buf_sz);
+ if(!buf){
+ fprintf(stderr, "memory allocation in sba_Axb_LU() failed!\n");
+ exit(1);
+ }
+ }
+
+ if(!iscolmaj){
+ a=buf;
+ ipiv=(int *)(a+a_sz);
+
+ /* store A (column major!) into a and B into x */
+ for(i=0; i<m; ++i){
+ for(j=0; j<m; ++j)
+ a[i+j*m]=A[i*m+j];
+
+ x[i]=B[i];
+ }
+ }
+ else{ /* no copying is necessary for A */
+ a=A;
+ for(i=0; i<m; ++i)
+ x[i]=B[i];
+ ipiv=(int *)buf;
+ }
+
+ /* LU decomposition for A */
+ F77_FUNC(dgetrf)((int *)&m, (int *)&m, a, (int *)&m, ipiv, (int *)&info);
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "argument %d of dgetrf illegal in sba_Axb_LU()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "singular matrix A for dgetrf in sba_Axb_LU()\n");
+ return 0;
+ }
+ }
+
+ /* solve the system with the computed LU */
+ F77_FUNC(dgetrs)("N", (int *)&m, (int *)&nrhs, a, (int *)&m, ipiv, x, (int *)&m, (int *)&info);
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "argument %d of dgetrs illegal in sba_Axb_LU()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "unknown error for dgetrs in sba_Axb_LU()\n");
+ return 0;
+ }
+ }
+
+ return 1;
+}
+
+/*
+ * This function returns the solution of Ax = b
+ *
+ * The function is based on SVD decomposition:
+ * If A=U D V^T with U, V orthogonal and D diagonal, the linear system becomes
+ * (U D V^T) x = b or x=V D^{-1} U^T b
+ * Note that V D^{-1} U^T is the pseudoinverse A^+
+ *
+ * A is mxm, b is mx1. Argument iscolmaj specifies whether A is
+ * stored in column or row major order. Note that if iscolmaj==1
+ * this function modifies A!
+ *
+ * The function returns 0 in case of error, 1 if successfull
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int sba_Axb_SVD(double *A, double *B, double *x, int m, int iscolmaj)
+{
+static double *buf=NULL;
+static int buf_sz=0;
+static double eps=-1.0;
+
+register int i, j;
+double *a, *u, *s, *vt, *work;
+int a_sz, u_sz, s_sz, vt_sz, tot_sz;
+double thresh, one_over_denom;
+register double sum;
+int info, rank, worksz, *iwork, iworksz;
+
+ if(A==NULL){
+ if(buf) free(buf);
+ buf=NULL;
+ buf_sz=0;
+
+ return 1;
+ }
+
+ /* calculate required memory size */
+#ifndef SBA_LS_SCARCE_MEMORY
+ worksz=-1; // workspace query. Keep in mind that dgesdd requires more memory than dgesvd
+ /* note that optimal work size is returned in thresh */
+ F77_FUNC(dgesdd)("A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m,
+ (double *)&thresh, (int *)&worksz, NULL, &info);
+ /* F77_FUNC(dgesvd)("A", "A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m,
+ (double *)&thresh, (int *)&worksz, &info); */
+ worksz=(int)thresh;
+#else
+ worksz=m*(7*m+4); // min worksize for dgesdd
+ //worksz=5*m; // min worksize for dgesvd
+#endif /* SBA_LS_SCARCE_MEMORY */
+ iworksz=8*m;
+ a_sz=(!iscolmaj)? m*m : 0;
+ u_sz=m*m; s_sz=m; vt_sz=m*m;
+
+ tot_sz=(a_sz + u_sz + s_sz + vt_sz + worksz)*sizeof(double) + iworksz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
+
+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+ if(buf) free(buf); /* free previously allocated memory */
+
+ buf_sz=tot_sz;
+ buf=(double *)malloc(buf_sz);
+ if(!buf){
+ fprintf(stderr, "memory allocation in sba_Axb_SVD() failed!\n");
+ exit(1);
+ }
+ }
+
+ if(!iscolmaj){
+ a=buf;
+ u=a+a_sz;
+ /* store A (column major!) into a */
+ for(i=0; i<m; ++i)
+ for(j=0; j<m; ++j)
+ a[i+j*m]=A[i*m+j];
+ }
+ else{
+ a=A; /* no copying required */
+ u=buf;
+ }
+
+ s=u+u_sz;
+ vt=s+s_sz;
+ work=vt+vt_sz;
+ iwork=(int *)(work+worksz);
+
+ /* SVD decomposition of A */
+ F77_FUNC(dgesdd)("A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, iwork, &info);
+ //F77_FUNC(dgesvd)("A", "A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, &info);
+
+ /* error treatment */
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "LAPACK error: illegal value for argument %d of dgesdd/dgesvd in sba_Axb_SVD()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "LAPACK error: dgesdd (dbdsdc)/dgesvd (dbdsqr) failed to converge in sba_Axb_SVD() [info=%d]\n", info);
+
+ return 0;
+ }
+ }
+
+ if(eps<0.0){
+ double aux;
+
+ /* compute machine epsilon. DBL_EPSILON should do also */
+ for(eps=1.0; aux=eps+1.0, aux-1.0>0.0; eps*=0.5)
+ ;
+ eps*=2.0;
+ }
+
+ /* compute the pseudoinverse in a */
+ memset(a, 0, m*m*sizeof(double)); /* initialize to zero */
+ for(rank=0, thresh=eps*s[0]; rank<m && s[rank]>thresh; ++rank){
+ one_over_denom=1.0/s[rank];
+
+ for(j=0; j<m; ++j)
+ for(i=0; i<m; ++i)
+ a[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;
+ }
+
+ /* compute A^+ b in x */
+ for(i=0; i<m; ++i){
+ for(j=0, sum=0.0; j<m; ++j)
+ sum+=a[i*m+j]*B[j];
+ x[i]=sum;
+ }
+
+ return 1;
+}
+
+/*
+ * This function returns the solution of Ax = b for a real symmetric matrix A
+ *
+ * The function is based on UDUT factorization with the pivoting
+ * strategy of Bunch and Kaufman:
+ * A is factored as U*D*U^T where U is upper triangular and
+ * D symmetric and block diagonal (aka spectral decomposition,
+ * Banachiewicz factorization, modified Cholesky factorization)
+ *
+ * A is mxm, b is mx1. Argument iscolmaj specifies whether A is
+ * stored in column or row major order. Note that if iscolmaj==1
+ * this function modifies A!
+ *
+ * The function returns 0 in case of error,
+ * 1 if successfull
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int sba_Axb_BK(double *A, double *B, double *x, int m, int iscolmaj)
+{
+static double *buf=NULL;
+static int buf_sz=0, nb=0;
+
+int a_sz, ipiv_sz, work_sz, tot_sz;
+register int i, j;
+int info, *ipiv, nrhs=1;
+double *a, *work;
+
+ if(A==NULL){
+ if(buf) free(buf);
+ buf=NULL;
+ buf_sz=0;
+
+ return 1;
+ }
+
+ /* calculate required memory size */
+ ipiv_sz=m;
+ a_sz=(iscolmaj)? 0 : m*m;
+ if(!nb){
+#ifndef SBA_LS_SCARCE_MEMORY
+ double tmp;
+
+ work_sz=-1; // workspace query; optimal size is returned in tmp
+ F77_FUNC(dsytrf)("U", (int *)&m, NULL, (int *)&m, NULL, (double *)&tmp, (int *)&work_sz, (int *)&info);
+ nb=((int)tmp)/m; // optimal worksize is m*nb
+#else
+ nb=-1; // min worksize is 1
+#endif /* SBA_LS_SCARCE_MEMORY */
+ }
+ work_sz=(nb!=-1)? nb*m : 1;
+ tot_sz=(a_sz + work_sz)*sizeof(double) + ipiv_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
+
+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+ if(buf) free(buf); /* free previously allocated memory */
+
+ buf_sz=tot_sz;
+ buf=(double *)malloc(buf_sz);
+ if(!buf){
+ fprintf(stderr, "memory allocation in sba_Axb_BK() failed!\n");
+ exit(1);
+ }
+ }
+
+ if(!iscolmaj){
+ a=buf;
+ work=a+a_sz;
+
+ /* store A into a and B into x; A is assumed to be symmetric, hence
+ * the column and row major order representations are the same
+ */
+ for(i=0; i<m; ++i){
+ a[i]=A[i];
+ x[i]=B[i];
+ }
+ for(j=m*m; i<j; ++i) // copy remaining rows; note that i is not re-initialized
+ a[i]=A[i];
+ }
+ else{ /* no copying is necessary for A */
+ a=A;
+ for(i=0; i<m; ++i)
+ x[i]=B[i];
+ work=buf;
+ }
+ ipiv=(int *)(work+work_sz);
+
+ /* factorization for A */
+ F77_FUNC(dsytrf)("U", (int *)&m, a, (int *)&m, ipiv, work, (int *)&work_sz, (int *)&info);
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "argument %d of dsytrf illegal in sba_Axb_BK()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "singular block diagonal matrix D for dsytrf in sba_Axb_BK() [D(%d, %d) is zero]\n", info, info);
+ return 0;
+ }
+ }
+
+ /* solve the system with the computed factorization */
+ F77_FUNC(dsytrs)("U", (int *)&m, (int *)&nrhs, a, (int *)&m, ipiv, x, (int *)&m, (int *)&info);
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "argument %d of dsytrs illegal in sba_Axb_BK()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "unknown error for dsytrs in sba_Axb_BK()\n");
+ return 0;
+ }
+ }
+
+ return 1;
+}
+
+/*
+ * This function computes the inverse of a square matrix whose upper triangle
+ * is stored in A into its lower triangle using LU decomposition
+ *
+ * The function returns 0 in case of error (e.g. A is singular),
+ * 1 if successfull
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int sba_symat_invert_LU(double *A, int m)
+{
+static double *buf=NULL;
+static int buf_sz=0, nb=0;
+
+int a_sz, ipiv_sz, work_sz, tot_sz;
+register int i, j;
+int info, *ipiv;
+double *a, *work;
+
+ if(A==NULL){
+ if(buf) free(buf);
+ buf=NULL;
+ buf_sz=0;
+
+ return 1;
+ }
+
+ /* calculate required memory size */
+ ipiv_sz=m;
+ a_sz=m*m;
+ if(!nb){
+#ifndef SBA_LS_SCARCE_MEMORY
+ double tmp;
+
+ work_sz=-1; // workspace query; optimal size is returned in tmp
+ F77_FUNC(dgetri)((int *)&m, NULL, (int *)&m, NULL, (double *)&tmp, (int *)&work_sz, (int *)&info);
+ nb=((int)tmp)/m; // optimal worksize is m*nb
+#else
+ nb=1; // min worksize is m
+#endif /* SBA_LS_SCARCE_MEMORY */
+ }
+ work_sz=nb*m;
+ tot_sz=(a_sz + work_sz)*sizeof(double) + ipiv_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
+
+ if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+ if(buf) free(buf); /* free previously allocated memory */
+
+ buf_sz=tot_sz;
+ buf=(double *)malloc(buf_sz);
+ if(!buf){
+ fprintf(stderr, "memory allocation in sba_symat_invert_LU() failed!\n");
+ exit(1);
+ }
+ }
+
+ a=buf;
+ work=a+a_sz;
+ ipiv=(int *)(work+work_sz);
+
+ /* store A (column major!) into a */
+ for(i=0; i<m; ++i)
+ for(j=i; j<m; ++j)
+ a[i+j*m]=a[j+i*m]=A[i*m+j]; // copy A's upper part to a's upper & lower
+
+ /* LU decomposition for A */
+ F77_FUNC(dgetrf)((int *)&m, (int *)&m, a, (int *)&m, ipiv, (int *)&info);
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "argument %d of dgetrf illegal in sba_symat_invert_LU()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "singular matrix A for dgetrf in sba_symat_invert_LU()\n");
+ return 0;
+ }
+ }
+
+ /* (A)^{-1} from LU */
+ F77_FUNC(dgetri)((int *)&m, a, (int *)&m, ipiv, work, (int *)&work_sz, (int *)&info);
+ if(info!=0){
+ if(info<0){
+ fprintf(stderr, "argument %d of dgetri illegal in sba_symat_invert_LU()\n", -info);
+ exit(1);
+ }
+ else{
+ fprintf(stderr, "singular matrix A for dgetri in sba_symat_invert_LU()\n");
+ return 0;
+ }
+ }
+
+ /* store (A)^{-1} in A's lower triangle */
+ for(i=0; i<m; ++i)
+ for(j=0; j<=i; ++j)
+ A[i*m+j]=a[i+j*m];
+
+ return 1;
+}
+
+/*
+ * This function computes the inverse of a square symmetric positive definite
+ * matrix whose upper triangle is stored in A into its lower triangle using
+ * Cholesky factorization
+ *
+ * The function returns 0 in case of error (e.g. A is not positive definite or singular),
+ * 1 if successfull
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int sba_symat_invert_Chol(double *A, int m)
+{
+static double *buf=NULL;
+static int buf_sz=0;
+
+int a_sz,