diff options
Diffstat (limited to 'libmoped')
| -rw-r--r-- | libmoped/libs/sba-1.6/CMakeLists.txt | 33 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/LICENSE | 340 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/Makefile | 34 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/Makefile.icc | 39 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/Makefile.vc | 47 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/README.txt | 65 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/compiler.h | 42 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/sba.h | 155 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/sba_chkjac.c | 458 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/sba_chkjac.h | 67 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/sba_crsm.c | 346 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/sba_lapack.c | 1678 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/sba_levmar.c | 2528 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/sba_levmar_wrap.c | 896 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/utils/README.txt | 37 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/utils/SBAio.pm | 239 | ||||
| -rw-r--r-- | libmoped/libs/sba-1.6/utils/hess2eps.c | 144 | ||||
| -rwxr-xr-x | libmoped/libs/sba-1.6/utils/quats.pl | 122 | ||||
| -rwxr-xr-x | libmoped/libs/sba-1.6/utils/reprerr.pl | 523 |
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, tot_sz; +register int i, j; +int info; +double *a; + + if(A==NULL){ + if(buf) free(buf); + buf=NULL; + buf_sz=0; + + return 1; + } + + /* calculate required memory size */ + a_sz=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_symat_invert_Chol() failed!\n"); + exit(1); + } + } + + a=(double *)buf; + + /* store A into a; A is assumed symmetric, hence no transposition is needed */ + for(i=0, j=a_sz; i<j; ++i) + a[i]=A[i]; + + /* Cholesky factorization for A; a's lower part corresponds to A's upper */ + //F77_FUNC(dpotrf)("L", (int *)&m, a, (int *)&m, (int *)&info); + F77_FUNC(dpotf2)("L", (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 dpotrf in sba_symat_invert_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 dpotrf in sba_symat_invert_Chol()\n", info); + return 0; + } + } + + /* (A)^{-1} from Cholesky */ + F77_FUNC(dpotri)("L", (int *)&m, a, (int *)&m, (int *)&info); + if(info!=0){ + if(info<0){ + fprintf(stderr, "argument %d of dpotri illegal in sba_symat_invert_Chol()\n", -info); + exit(1); + } + else{ + fprintf(stderr, "the (%d, %d) element of the factor U or L is zero, singular matrix A for dpotri in sba_symat_invert_Chol()\n", info |