diff options
Diffstat (limited to 'libmoped/libs')
| -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, info); + return 0; + } + } + + /* store (A)^{-1} in A's lower triangle. The lower triangle of the symmetric A^{-1} is in the lower triangle of a */ + 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 symmetric indefinite + * matrix whose upper triangle is stored in A into its lower triangle + * using LDLT factorization with the pivoting strategy of Bunch and Kaufman + * + * 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_BK(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(dsytrf)("L", (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; + work_sz=(work_sz>=m)? work_sz : m; /* ensure that work is at least m elements long, as required by dsytri */ + 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_BK() failed!\n"); + exit(1); + } + } + + a=buf; + work=a+a_sz; + ipiv=(int *)(work+work_sz); + + /* 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]; + + /* LDLT factorization for A; a's lower part corresponds to A's upper */ + F77_FUNC(dsytrf)("L", (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_symat_invert_BK()\n", -info); + exit(1); + } + else{ + fprintf(stderr, "singular block diagonal matrix D for dsytrf in sba_symat_invert_BK() [D(%d, %d) is zero]\n", info, info); + return 0; + } + } + + /* (A)^{-1} from LDLT */ + F77_FUNC(dsytri)("L", (int *)&m, a, (int *)&m, ipiv, work, (int *)&info); + if(info!=0){ + if(info<0){ + fprintf(stderr, "argument %d of dsytri illegal in sba_symat_invert_BK()\n", -info); + exit(1); + } + else{ + fprintf(stderr, "D(%d, %d)=0, matrix is singular and its inverse could not be computed in sba_symat_invert_BK()\n", info, info); + return 0; + } + } + + /* store (A)^{-1} in A's lower triangle. The lower triangle of the symmetric A^{-1} is in the lower triangle of a */ + for(i=0; i<m; ++i) + for(j=0; j<=i; ++j) + A[i*m+j]=a[i+j*m]; + + return 1; +} + + +#define __CG_LINALG_BLOCKSIZE 8 + +/* Dot product of two vectors x and y using loop unrolling and blocking. + * see http://www.abarnett.demon.co.uk/tutorial.html + */ + +inline static double dprod(const int n, const double *const x, const double *const y) +{ +register int i, j1, j2, j3, j4, j5, j6, j7; +int blockn; +register double sum0=0.0, sum1=0.0, sum2=0.0, sum3=0.0, + sum4=0.0, sum5=0.0, sum6=0.0, sum7=0.0; + + /* n may not be divisible by __CG_LINALG_BLOCKSIZE, + * go as near as we can first, then tidy up. + */ + blockn = (n / __CG_LINALG_BLOCKSIZE) * __CG_LINALG_BLOCKSIZE; + + /* unroll the loop in blocks of `__CG_LINALG_BLOCKSIZE' */ + for(i=0; i<blockn; i+=__CG_LINALG_BLOCKSIZE){ + sum0+=x[i]*y[i]; + j1=i+1; sum1+=x[j1]*y[j1]; + j2=i+2; sum2+=x[j2]*y[j2]; + j3=i+3; sum3+=x[j3]*y[j3]; + j4=i+4; sum4+=x[j4]*y[j4]; + j5=i+5; sum5+=x[j5]*y[j5]; + j6=i+6; sum6+=x[j6]*y[j6]; + j7=i+7; sum7+=x[j7]*y[j7]; + } + + /* + * There may be some left to do. + * This could be done as a simple for() loop, + * but a switch is faster (and more interesting) + */ + + if(i<n){ + /* Jump into the case at the place that will allow + * us to finish off the appropriate number of items. + */ + + switch(n - i){ + case 7 : sum0+=x[i]*y[i]; ++i; + case 6 : sum1+=x[i]*y[i]; ++i; + case 5 : sum2+=x[i]*y[i]; ++i; + case 4 : sum3+=x[i]*y[i]; ++i; + case 3 : sum4+=x[i]*y[i]; ++i; + case 2 : sum5+=x[i]*y[i]; ++i; + case 1 : sum6+=x[i]*y[i]; ++i; + } + } + + return sum0+sum1+sum2+sum3+sum4+sum5+sum6+sum7; +} + + +/* Compute z=x+a*y for two vectors x and y and a scalar a; z can be one of x, y. + * Similarly to the dot product routine, this one uses loop unrolling and blocking + */ + +inline static void daxpy(const int n, double *const z, const double *const x, const double a, const double *const y) +{ +register int i, j1, j2, j3, j4, j5, j6, j7; +int blockn; + + /* n may not be divisible by __CG_LINALG_BLOCKSIZE, + * go as near as we can first, then tidy up. + */ + blockn = (n / __CG_LINALG_BLOCKSIZE) * __CG_LINALG_BLOCKSIZE; + + /* unroll the loop in blocks of `__CG_LINALG_BLOCKSIZE' */ + for(i=0; i<blockn; i+=__CG_LINALG_BLOCKSIZE){ + z[i]=x[i]+a*y[i]; + j1=i+1; z[j1]=x[j1]+a*y[j1]; + j2=i+2; z[j2]=x[j2]+a*y[j2]; + j3=i+3; z[j3]=x[j3]+a*y[j3]; + j4=i+4; z[j4]=x[j4]+a*y[j4]; + j5=i+5; z[j5]=x[j5]+a*y[j5]; + j6=i+6; z[j6]=x[j6]+a*y[j6]; + j7=i+7; z[j7]=x[j7]+a*y[j7]; + } + + /* + * There may be some left to do. + * This could be done as a simple for() loop, + * but a switch is faster (and more interesting) + */ + + if(i<n){ + /* Jump into the case at the place that will allow + * us to finish off the appropriate number of items. + */ + + switch(n - i){ + case 7 : z[i]=x[i]+a*y[i]; ++i; + case 6 : z[i]=x[i]+a*y[i]; ++i; + case 5 : z[i]=x[i]+a*y[i]; ++i; + case 4 : z[i]=x[i]+a*y[i]; ++i; + case 3 : z[i]=x[i]+a*y[i]; ++i; + case 2 : z[i]=x[i]+a*y[i]; ++i; + case 1 : z[i]=x[i]+a*y[i]; ++i; + } + } +} + +/* + * This function returns the solution of Ax = b where A is posititive definite, + * based on the conjugate gradients method; see "An intro to the CG method" by J.R. Shewchuk, p. 50-51 + * + * A is mxm, b, x are is mx1. Argument niter specifies the maximum number of + * iterations and eps is the desired solution accuracy. niter<0 signals that + * x contains a valid initial approximation to the solution; if niter>0 then + * the starting point is taken to be zero. Argument prec selects the desired + * preconditioning method as follows: + * 0: no preconditioning + * 1: jacobi (diagonal) preconditioning + * 2: SSOR preconditioning + * Argument iscolmaj specifies whether A is stored in column or row major order. + * + * The function returns 0 in case of error, + * the number of iterations performed 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_CG(double *A, double *B, double *x, int m, int niter, double eps, int prec, int iscolmaj) +{ +static double *buf=NULL; +static int buf_sz=0; + +register int i, j; +register double *aim; +int iter, a_sz, res_sz, d_sz, q_sz, s_sz, wk_sz, z_sz, tot_sz; +double *a, *res, *d, *q, *s, *wk, *z; +double delta0, deltaold, deltanew, alpha, beta, eps_sq=eps*eps; +register double sum; +int rec_res; + + if(A==NULL){ + if(buf) free(buf); + buf=NULL; + buf_sz=0; + + return 1; + } + + /* calculate required memory size */ + a_sz=(iscolmaj)? m*m : 0; + res_sz=m; d_sz=m; q_sz=m; + if(prec!=SBA_CG_NOPREC){ + s_sz=m; wk_sz=m; + z_sz=(prec==SBA_CG_SSOR)? m : 0; + } + else + s_sz=wk_sz=z_sz=0; + + tot_sz=a_sz+res_sz+d_sz+q_sz+s_sz+wk_sz+z_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 request failed in sba_Axb_CG()\n"); + exit(1); + } + } + + if(iscolmaj){ + a=buf; + /* store A (row major!) into a */ + for(i=0; i<m; ++i) + for(j=0, aim=a+i*m; j<m; ++j) + aim[j]=A[i+j*m]; + } + else a=A; /* no copying required */ + + res=buf+a_sz; + d=res+res_sz; + q=d+d_sz; + if(prec!=SBA_CG_NOPREC){ + s=q+q_sz; + wk=s+s_sz; + z=(prec==SBA_CG_SSOR)? wk+wk_sz : NULL; + + for(i=0; i<m; ++i){ // compute jacobi (i.e. diagonal) preconditioners and save them in wk + sum=a[i*m+i]; + if(sum>DBL_EPSILON || -sum<-DBL_EPSILON) // != 0.0 + wk[i]=1.0/sum; + else + wk[i]=1.0/DBL_EPSILON; + } + } + else{ + s=res; + wk=z=NULL; + } + + if(niter>0){ + for(i=0; i<m; ++i){ // clear solution and initialize residual vector: res <-- B + x[i]=0.0; + res[i]=B[i]; + } + } + else{ + niter=-niter; + + for(i=0; i<m; ++i){ // initialize residual vector: res <-- B - A*x + for(j=0, aim=a+i*m, sum=0.0; j<m; ++j) + sum+=aim[j]*x[j]; + res[i]=B[i]-sum; + } + } + + switch(prec){ + case SBA_CG_NOPREC: + for(i=0, deltanew=0.0; i<m; ++i){ + d[i]=res[i]; + deltanew+=res[i]*res[i]; + } + break; + case SBA_CG_JACOBI: // jacobi preconditioning + for(i=0, deltanew=0.0; i<m; ++i){ + d[i]=res[i]*wk[i]; + deltanew+=res[i]*d[i]; + } + break; + case SBA_CG_SSOR: // SSOR preconditioning; see the "templates" book, fig. 3.2, p. 44 + for(i=0; i<m; ++i){ + for(j=0, sum=0.0, aim=a+i*m; j<i; ++j) + sum+=aim[j]*z[j]; + z[i]=wk[i]*(res[i]-sum); + } + + for(i=m-1; i>=0; --i){ + for(j=i+1, sum=0.0, aim=a+i*m; j<m; ++j) + sum+=aim[j]*d[j]; + d[i]=z[i]-wk[i]*sum; + } + deltanew=dprod(m, res, d); + break; + default: + fprintf(stderr, "unknown preconditioning option %d in sba_Axb_CG\n", prec); + exit(1); + } + + delta0=deltanew; + + for(iter=1; deltanew>eps_sq*delta0 && iter<=niter; ++iter){ + for(i=0; i<m; ++i){ // q <-- A d + aim=a+i*m; +/*** + for(j=0, sum=0.0; j<m; ++j) + sum+=aim[j]*d[j]; +***/ + q[i]=dprod(m, aim, d); //sum; + } + +/*** + for(i=0, sum=0.0; i<m; ++i) + sum+=d[i]*q[i]; +***/ + alpha=deltanew/dprod(m, d, q); // deltanew/sum; + +/*** + for(i=0; i<m; ++i) + x[i]+=alpha*d[i]; +***/ + daxpy(m, x, x, alpha, d); + + if(!(iter%50)){ + for(i=0; i<m; ++i){ // accurate computation of the residual vector + aim=a+i*m; +/*** + for(j=0, sum=0.0; j<m; ++j) + sum+=aim[j]*x[j]; +***/ + res[i]=B[i]-dprod(m, aim, x); //B[i]-sum; + } + rec_res=0; + } + else{ +/*** + for(i=0; i<m; ++i) // approximate computation of the residual vector + res[i]-=alpha*q[i]; +***/ + daxpy(m, res, res, -alpha, q); + rec_res=1; + } + + if(prec){ + switch(prec){ + case SBA_CG_JACOBI: // jacobi + for(i=0; i<m; ++i) + s[i]=res[i]*wk[i]; + break; + case SBA_CG_SSOR: // SSOR + for(i=0; i<m; ++i){ + for(j=0, sum=0.0, aim=a+i*m; j<i; ++j) + sum+=aim[j]*z[j]; + z[i]=wk[i]*(res[i]-sum); + } + + for(i=m-1; i>=0; --i){ + for(j=i+1, sum=0.0, aim=a+i*m; j<m; ++j) + sum+=aim[j]*s[j]; + s[i]=z[i]-wk[i]*sum; + } + break; + } + } + + deltaold=deltanew; +/*** + for(i=0, sum=0.0; i<m; ++i) + sum+=res[i]*s[i]; +***/ + deltanew=dprod(m, res, s); //sum; + + /* make sure that we get around small delta that are due to + * accumulated floating point roundoff errors + */ + if(rec_res && deltanew<=eps_sq*delta0){ + /* analytically recompute delta */ + for(i=0; i<m; ++i){ + for(j=0, aim=a+i*m, sum=0.0; j<m; ++j) + sum+=aim[j]*x[j]; + res[i]=B[i]-sum; + } + deltanew=dprod(m, res, s); + } + + beta=deltanew/deltaold; + +/*** + for(i=0; i<m; ++i) + d[i]=s[i]+beta*d[i]; +***/ + daxpy(m, d, s, beta, d); + } + + return iter; +} + +/* + * This function computes the Cholesky decomposition of the inverse of a symmetric + * (covariance) matrix A into B, i.e. B is s.t. A^-1=B^t*B and B upper triangular. + * A and B can coincide + * + * The function returns 0 in case of error (e.g. A is singular), + * 1 if successfull + * + * This function is often called repetitively to operate on matrices 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. + * + */ +#if 0 +int sba_mat_cholinv(double *A, double *B, 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 the 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_mat_cholinv() failed!\n"); + exit(1); + } + } + + a=buf; + work=a+a_sz; + ipiv=(int *)(work+work_sz); + + /* step 1: invert A */ + /* store A into a; A is assumed symmetric, hence no transposition is needed */ + for(i=0; i<m*m; ++i) + a[i]=A[i]; + + /* LU decomposition for A (Cholesky should also do) */ + F77_FUNC(dgetf2)((int *)&m, (int *)&m, a, (int *)&m, ipiv, (int *)&info); + //F77_FUNC(dgetrf)((int *)&m, (int *)&m, a, (int *)&m, ipiv, (int *)&info); + if(info!=0){ + if(info<0){ + fprintf(stderr, "argument %d of dgetf2/dgetrf illegal in sba_mat_cholinv()\n", -info); + exit(1); + } + else{ + fprintf(stderr, "singular matrix A for dgetf2/dgetrf in sba_mat_cholinv()\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_mat_cholinv()\n", -info); + exit(1); + } + else{ + fprintf(stderr, "singular matrix A for dgetri in sba_mat_cholinv()\n"); + return 0; + } + } + + /* (A)^{-1} is now in a (in column major!) */ + + /* step 2: Cholesky decomposition of a: A^-1=B^t B, B upper triangular */ + F77_FUNC(dpotf2)("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 in sba_mat_cholinv()\n", -info); + exit(1); + } + else{ + fprintf(stderr, "LAPACK error: the leading minor of order %d is not positive definite,\n%s\n", info, + "and the Cholesky factorization could not be completed in sba_mat_cholinv()"); + return 0; + } + } + + /* the decomposition is in the upper part of a (in column-major order!). + * copying it to the lower part and zeroing the upper transposes + * a in row-major order + */ + for(i=0; i<m; ++i) + for(j=0; j<i; ++j){ + a[i+j*m]=a[j+i*m]; + a[j+i*m]=0.0; + } + for(i=0; i<m*m; ++i) + B[i]=a[i]; + + return 1; +} +#endif + +int sba_mat_cholinv(double *A, double *B, int m) +{ +int a_sz; +register int i, j; +int info; +double *a; + + if(A==NULL){ + return 1; + } + + a_sz=m*m; + a=B; /* use B as working memory, result is produced in it */ + + /* step 1: invert A */ + /* store A into a; A is assumed symmetric, hence no transposition is needed */ + for(i=0; i<a_sz; ++i) + a[i]=A[i]; + + /* Cholesky decomposition for A */ + F77_FUNC(dpotf2)("U", (int *)&m, a, (int *)&m, (int *)&info); + if(info!=0){ + if(info<0){ + fprintf(stderr, "argument %d of dpotf2 illegal in sba_mat_cholinv()\n", -info); + exit(1); + } + else{ + fprintf(stderr, "LAPACK error: the leading minor of order %d is not positive definite,\n%s\n", info, + "and the Cholesky factorization could not be completed in sba_mat_cholinv()"); + return 0; + } + } + + /* (A)^{-1} from Cholesky */ + F77_FUNC(dpotri)("U", (int *)&m, a, (int *)&m, (int *)&info); + if(info!=0){ + if(info<0){ + fprintf(stderr, "argument %d of dpotri illegal in sba_mat_cholinv()\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_mat_cholinv()\n", info, info); + return 0; + } + } + + /* (A)^{-1} is now in a (in column major!) */ + + /* step 2: Cholesky decomposition of a: A^-1=B^t B, B upper triangular */ + F77_FUNC(dpotf2)("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 in sba_mat_cholinv()\n", -info); + exit(1); + } + else{ + fprintf(stderr, "LAPACK error: the leading minor of order %d is not positive definite,\n%s\n", info, + "and the Cholesky factorization could not be completed in sba_mat_cholinv()"); + return 0; + } + } + + /* the decomposition is in the upper part of a (in column-major order!). + * copying it to the lower part and zeroing the upper transposes + * a in row-major order + */ + for(i=0; i<m; ++i) + for(j=0; j<i; ++j){ + a[i+j*m]=a[j+i*m]; + a[j+i*m]=0.0; + } + + return 1; +} diff --git a/libmoped/libs/sba-1.6/sba_levmar.c b/libmoped/libs/sba-1.6/sba_levmar.c new file mode 100644 index 0000000..aa6b2e9 --- /dev/null +++ b/libmoped/libs/sba-1.6/sba_levmar.c @@ -0,0 +1,2528 @@ +///////////////////////////////////////////////////////////////////////////////// +//// +//// Expert drivers for sparse bundle adjustment based on the +//// Levenberg - Marquardt minimization algorithm +//// 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. +//// +/////////////////////////////////////////////////////////////////////////////////// + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> +#include <float.h> + +#include "compiler.h" +#include "sba.h" +#include "sba_chkjac.h" + + +#define SBA_EPSILON 1E-12 +#define SBA_EPSILON_SQ ( (SBA_EPSILON)*(SBA_EPSILON) ) + +#define SBA_ONE_THIRD 0.3333333334 /* 1.0/3.0 */ + + +#define emalloc(sz) emalloc_(__FILE__, __LINE__, sz) + +#define FABS(x) (((x)>=0)? (x) : -(x)) + +#define ROW_MAJOR 0 +#define COLUMN_MAJOR 1 +#define MAT_STORAGE COLUMN_MAJOR + + +/* contains information necessary for computing a finite difference approximation to a jacobian, + * e.g. function to differentiate, problem dimensions and pointers to working memory buffers + */ +struct fdj_data_x_ { + void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata); /* function to differentiate */ + int cnp, pnp, mnp; /* parameter numbers */ + int *func_rcidxs, + *func_rcsubs; /* working memory for func invocations. + * Notice that this has to be different + * than the working memory used for + * evaluating the jacobian! + */ + double *hx, *hxx; /* memory to save results in */ + void *adata; +}; + +/* 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; +} + +/* auxiliary routine for clearing an array of doubles */ +inline static void _dblzero(register double *arr, register int count) +{ + while(--count>=0) + *arr++=0.0; +} + +/* auxiliary routine for computing the mean reprojection error; used for debugging */ +static double sba_mean_repr_error(int n, int mnp, double *x, double *hx, struct sba_crsm *idxij, int *rcidxs, int *rcsubs) +{ +register int i, j; +int nnz, nprojs; +double *ptr1, *ptr2; +double err; + + for(i=nprojs=0, err=0.0; i<n; ++i){ + nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero x_ij, j=0...m-1 */ + nprojs+=nnz; + for(j=0; j<nnz; ++j){ /* point i projecting on camera rcsubs[j] */ + ptr1=x + idxij->val[rcidxs[j]]*mnp; + ptr2=hx + idxij->val[rcidxs[j]]*mnp; + + err+=sqrt((ptr1[0]-ptr2[0])*(ptr1[0]-ptr2[0]) + (ptr1[1]-ptr2[1])*(ptr1[1]-ptr2[1])); + } + } + + return err/((double)(nprojs)); +} + +/* print the solution in p using sba's text format. If cnp/pnp==0 only points/cameras are printed */ +static void sba_print_sol(int n, int m, double *p, int cnp, int pnp, double *x, int mnp, struct sba_crsm *idxij, int *rcidxs, int *rcsubs) +{ +register int i, j, ii; +int nnz; +double *ptr; + + if(cnp){ + /* print camera parameters */ + for(j=0; j<m; ++j){ + ptr=p+cnp*j; + for(ii=0; ii<cnp; ++ii) + printf("%g ", ptr[ii]); + printf("\n"); + } + } + + if(pnp){ + /* 3D & 2D point parameters */ + printf("\n\n\n# X Y Z nframes frame0 x0 y0 frame1 x1 y1 ...\n"); + for(i=0; i<n; ++i){ + ptr=p+cnp*m+i*pnp; + for(ii=0; ii<pnp; ++ii) // print 3D coordinates + printf("%g ", ptr[ii]); + + nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero x_ij, j=0...m-1 */ + printf("%d ", nnz); + + for(j=0; j<nnz; ++j){ /* point i projecting on camera rcsubs[j] */ + ptr=x + idxij->val[rcidxs[j]]*mnp; + + printf("%d ", rcsubs[j]); + for(ii=0; ii<mnp; ++ii) // print 2D coordinates + printf("%g ", ptr[ii]); + } + printf("\n"); + } + } +} + +/* Compute e=x-y for two n-vectors x and y and return the squared L2 norm of e. + * e can coincide with either x or y. + * Uses loop unrolling and blocking to reduce bookkeeping overhead & pipeline + * stalls and increase instruction-level parallelism; see http://www.abarnett.demon.co.uk/tutorial.html + */ +static double nrmL2xmy(double *const e, const double *const x, const double *const y, const int n) +{ +const int blocksize=8, bpwr=3; /* 8=2^3 */ +register int i; +int j1, j2, j3, j4, j5, j6, j7; +int blockn; +register double sum0=0.0, sum1=0.0, sum2=0.0, sum3=0.0; + + /* n may not be divisible by blocksize, + * go as near as we can first, then tidy up. + */ + blockn = (n>>bpwr)<<bpwr; /* (n / blocksize) * blocksize; */ + + /* unroll the loop in blocks of `blocksize'; looping downwards gains some more speed */ + for(i=blockn-1; i>0; i-=blocksize){ + e[i ]=x[i ]-y[i ]; sum0+=e[i ]*e[i ]; + j1=i-1; e[j1]=x[j1]-y[j1]; sum1+=e[j1]*e[j1]; + j2=i-2; e[j2]=x[j2]-y[j2]; sum2+=e[j2]*e[j2]; + j3=i-3; e[j3]=x[j3]-y[j3]; sum3+=e[j3]*e[j3]; + j4=i-4; e[j4]=x[j4]-y[j4]; sum0+=e[j4]*e[j4]; + j5=i-5; e[j5]=x[j5]-y[j5]; sum1+=e[j5]*e[j5]; + j6=i-6; e[j6]=x[j6]-y[j6]; sum2+=e[j6]*e[j6]; + j7=i-7; e[j7]=x[j7]-y[j7]; sum3+=e[j7]*e[j7]; + } + + /* + * There may be some left to do. + * This could be done as a simple for() loop, + * but a switch is faster (and more interesting) + */ + + i=blockn; + if(i<n){ + /* Jump into the case at the place that will allow + * us to finish off the appropriate number of items. + */ + switch(n - i){ + case 7 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i; + case 6 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i; + case 5 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i; + case 4 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i; + case 3 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i; + case 2 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i; + case 1 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i; + } + } + + return sum0+sum1+sum2+sum3; +} + +/* Compute e=W*(x-y) for two n-vectors x and y and return the squared L2 norm of e. + * This norm equals the squared C norm of x-y with C=W^T*W, W being block diagonal + * matrix with nvis mnp*mnp blocks: e^T*e=(x-y)^T*W^T*W*(x-y)=||x-y||_C. + * Note that n=nvis*mnp; e can coincide with either x or y. + * + * Similarly to nrmL2xmy() above, uses loop unrolling and blocking + */ +static double nrmCxmy(double *const e, const double *const x, const double *const y, + const double *const W, const int mnp, const int nvis) +{ +const int n=nvis*mnp; +const int blocksize=8, bpwr=3; /* 8=2^3 */ +register int i, ii, k; +int j1, j2, j3, j4, j5, j6, j7; +int blockn, mnpsq; +register double norm, sum; +register const double *Wptr, *auxptr; +register double *eptr; + + /* n may not be divisible by blocksize, + * go as near as we can first, then tidy up. + */ + blockn = (n>>bpwr)<<bpwr; /* (n / blocksize) * blocksize; */ + + /* unroll the loop in blocks of `blocksize'; looping downwards gains some more speed */ + for(i=blockn-1; i>0; i-=blocksize){ + e[i ]=x[i ]-y[i ]; + j1=i-1; e[j1]=x[j1]-y[j1]; + j2=i-2; e[j2]=x[j2]-y[j2]; + j3=i-3; e[j3]=x[j3]-y[j3]; + j4=i-4; e[j4]=x[j4]-y[j4]; + j5=i-5; e[j5]=x[j5]-y[j5]; + j6=i-6; e[j6]=x[j6]-y[j6]; + j7=i-7; e[j7]=x[j7]-y[j7]; + } + + /* + * There may be some left to do. + * This could be done as a simple for() loop, + * but a switch is faster (and more interesting) + */ + + i=blockn; + if(i<n){ + /* Jump into the case at the place that will allow + * us to finish off the appropriate number of items. + */ + switch(n - i){ + case 7 : e[i]=x[i]-y[i]; ++i; + case 6 : e[i]=x[i]-y[i]; ++i; + case 5 : e[i]=x[i]-y[i]; ++i; + case 4 : e[i]=x[i]-y[i]; ++i; + case 3 : e[i]=x[i]-y[i]; ++i; + case 2 : e[i]=x[i]-y[i]; ++i; + case 1 : e[i]=x[i]-y[i]; ++i; + } + } + + /* compute w_x_ij e_ij in e and its L2 norm. + * Since w_x_ij is upper triangular, the products can be safely saved + * directly in e_ij, without the need for intermediate storage + */ + mnpsq=mnp*mnp; + /* Wptr, eptr point to w_x_ij, e_ij below */ + for(i=0, Wptr=W, eptr=e, norm=0.0; i<nvis; ++i, Wptr+=mnpsq, eptr+=mnp){ + for(ii=0, auxptr=Wptr; ii<mnp; ++ii, auxptr+=mnp){ /* auxptr=Wptr+ii*mnp */ + for(k=ii, sum=0.0; k<mnp; ++k) // k>=ii since w_x_ij is upper triangular + sum+=auxptr[k]*eptr[k]; //Wptr[ii*mnp+k]*eptr[k]; + eptr[ii]=sum; + norm+=sum*sum; + } + } + + return norm; +} + +/* search for & print image projection components that are infinite; useful for identifying errors */ +static void sba_print_inf(double *hx, int nimgs, int mnp, struct sba_crsm *idxij, int *rcidxs, int *rcsubs) +{ +register int i, j, k; +int nnz; +double *phxij; + + for(j=0; j<nimgs; ++j){ + nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + for(i=0; i<nnz; ++i){ + phxij=hx + idxij->val[rcidxs[i]]*mnp; + for(k=0; k<mnp; ++k) + if(!SBA_FINITE(phxij[k])) + printf("SBA: component %d of the estimated projection of point %d on camera %d is invalid!\n", k, rcsubs[i], j); + } + } +} + +/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in + * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images. + * The jacobian is approximated with the aid of finite differences and is returned in the order + * (A_11, B_11, ..., A_1m, B_1m, ..., A_n1, B_n1, ..., A_nm, B_nm), + * where A_ij=dx_ij/da_j and B_ij=dx_ij/db_i (see HZ). + * Notice that depending on idxij, some of the A_ij, B_ij might be missing + * + * Problem-specific information is assumed to be stored in a structure pointed to by "dat". + * + * NOTE: The jacobian (for n=4, m=3) in matrix form has the following structure: + * A_11 0 0 B_11 0 0 0 + * 0 A_12 0 B_12 0 0 0 + * 0 0 A_13 B_13 0 0 0 + * A_21 0 0 0 B_21 0 0 + * 0 A_22 0 0 B_22 0 0 + * 0 0 A_23 0 B_23 0 0 + * A_31 0 0 0 0 B_31 0 + * 0 A_32 0 0 0 B_32 0 + * 0 0 A_33 0 0 B_33 0 + * A_41 0 0 0 0 0 B_41 + * 0 A_42 0 0 0 0 B_42 + * 0 0 A_43 0 0 0 B_43 + * To reduce the total number of objective function evaluations, this structure can be + * exploited as follows: A certain d is added to the j-th parameters of all cameras and + * the objective function is evaluated at the resulting point. This evaluation suffices + * to compute the corresponding columns of *all* A_ij through finite differences. A similar + * strategy allows the computation of the B_ij. Overall, only cnp+pnp+1 objective function + * evaluations are needed to compute the jacobian, much fewer compared to the m*cnp+n*pnp+1 + * that would be required by the naive strategy of computing one column of the jacobian + * per function evaluation. See Nocedal-Wright, ch. 7, pp. 169. Although this approach is + * much faster compared to the naive strategy, it is not preferable to analytic jacobians, + * since the latter are considerably faster to compute and result in fewer LM iterations. + */ +static void sba_fdjac_x( + double *p, /* I: current parameter estimate, (m*cnp+n*pnp)x1 */ + struct sba_crsm *idxij, /* I: sparse matrix containing the location of x_ij in hx */ + int *rcidxs, /* work array for the indexes of nonzero elements of a single sparse matrix row/column */ + int *rcsubs, /* work array for the subscripts of nonzero elements in a single sparse matrix row/column */ + double *jac, /* O: array for storing the approximated jacobian */ + void *dat) /* I: points to a "fdj_data_x_" structure */ +{ + register int i, j, ii, jj; + double *pa, *pb, *pqr, *ppt; + register double *pAB, *phx, *phxx; + int n, m, nm, nnz, Asz, Bsz, ABsz, idx; + + double *tmpd; + register double d; + + struct fdj_data_x_ *fdjd; + void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata); + double *hx, *hxx; + int cnp, pnp, mnp; + void *adata; + + + /* retrieve problem-specific information passed in *dat */ + fdjd=(struct fdj_data_x_ *)dat; + func=fdjd->func; + cnp=fdjd->cnp; pnp=fdjd->pnp; mnp=fdjd->mnp; + hx=fdjd->hx; + hxx=fdjd->hxx; + adata=fdjd->adata; + + n=idxij->nr; m=idxij->nc; + pa=p; pb=p+m*cnp; + Asz=mnp*cnp; Bsz=mnp*pnp; ABsz=Asz+Bsz; + + nm=(n>=m)? n : m; // max(n, m); + tmpd=(double *)emalloc(nm*sizeof(double)); + + (*func)(p, idxij, fdjd->func_rcidxs, fdjd->func_rcsubs, hx, adata); // evaluate supplied function on current solution + + if(cnp){ // is motion varying? + /* compute A_ij */ + for(jj=0; jj<cnp; ++jj){ + for(j=0; j<m; ++j){ + pqr=pa+j*cnp; // j-th camera parameters + /* determine d=max(SBA_DELTA_SCALE*|pqr[jj]|, SBA_MIN_DELTA), see HZ */ + d=(double)(SBA_DELTA_SCALE)*pqr[jj]; // force evaluation + d=FABS(d); + if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA; + + tmpd[j]=d; + pqr[jj]+=d; + } + + (*func)(p, idxij, fdjd->func_rcidxs, fdjd->func_rcsubs, hxx, adata); + + for(j=0; j<m; ++j){ + pqr=pa+j*cnp; // j-th camera parameters + pqr[jj]-=tmpd[j]; /* restore */ + d=1.0/tmpd[j]; /* invert so that divisions can be carried out faster as multiplications */ + + nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero A_ij, i=0...n-1 */ + for(i=0; i<nnz; ++i){ + idx=idxij->val[rcidxs[i]]; + phx=hx + idx*mnp; // set phx to point to hx_ij + phxx=hxx + idx*mnp; // set phxx to point to hxx_ij + pAB=jac + idx*ABsz; // set pAB to point to A_ij + + for(ii=0; ii<mnp; ++ii) + pAB[ii*cnp+jj]=(phxx[ii]-phx[ii])*d; + } + } + } + } + + if(pnp){ // is structure varying? + /* compute B_ij */ + for(jj=0; jj<pnp; ++jj){ + for(i=0; i<n; ++i){ + ppt=pb+i*pnp; // i-th point parameters + /* determine d=max(SBA_DELTA_SCALE*|ppt[jj]|, SBA_MIN_DELTA), see HZ */ + d=(double)(SBA_DELTA_SCALE)*ppt[jj]; // force evaluation + d=FABS(d); + if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA; + + tmpd[i]=d; + ppt[jj]+=d; + } + + (*func)(p, idxij, fdjd->func_rcidxs, fdjd->func_rcsubs, hxx, adata); + + for(i=0; i<n; ++i){ + ppt=pb+i*pnp; // i-th point parameters + ppt[jj]-=tmpd[i]; /* restore */ + d=1.0/tmpd[i]; /* invert so that divisions can be carried out faster as multiplications */ + + nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero B_ij, j=0...m-1 */ + for(j=0; j<nnz; ++j){ + idx=idxij->val[rcidxs[j]]; + phx=hx + idx*mnp; // set phx to point to hx_ij + phxx=hxx + idx*mnp; // set phxx to point to hxx_ij + pAB=jac + idx*ABsz + Asz; // set pAB to point to B_ij + + for(ii=0; ii<mnp; ++ii) + pAB[ii*pnp+jj]=(phxx[ii]-phx[ii])*d; + } + } + } + } + + free(tmpd); +} + +typedef int (*PLS)(double *A, double *B, double *x, int m, int iscolmaj); + +/* Bundle adjustment on camera and structure parameters + * using the sparse Levenberg-Marquardt as described in HZ p. 568 + * + * Returns the number of iterations (>=0) if successfull, SBA_ERROR if failed + */ + +int sba_motstr_levmar_x( + const int n, /* number of points */ + const int ncon,/* number of points (starting from the 1st) whose parameters should not be modified. + * All B_ij (see below) with i<ncon are assumed to be zero + */ + const int m, /* number of images */ + const int mcon,/* number of images (starting from the 1st) whose parameters should not be modified. + * All A_ij (see below) with j<mcon are assumed to be zero + */ + char *vmask, /* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */ + double *p, /* initial parameter vector p0: (a1, ..., am, b1, ..., bn). + * aj are the image j parameters, bi are the i-th point parameters, + * size m*cnp + n*pnp + */ + const int cnp,/* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */ + const int pnp,/* number of parameters for ONE point; e.g. 3 for Euclidean points */ + double *x, /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where + * x_ij is the projection of the i-th point on the j-th image. + * NOTE: some of the x_ij might be missing, if point i is not visible in image j; + * see vmask[i, j], max. size n*m*mnp + */ + double *covx, /* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm), + * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no + * covariance estimates are available (identity matrices are implicitly used in this case). + * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing; + * see vmask[i, j], max. size n*m*mnp*mnp + */ + const int mnp,/* number of parameters for EACH measurement; usually 2 */ + void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata), + /* functional relation describing measurements. Given a parameter vector p, + * computes a prediction of the measurements \hat{x}. p is (m*cnp + n*pnp)x1, + * \hat{x} is (n*m*mnp)x1, maximum + * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used + * as working memory + */ + void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata), + /* function to evaluate the sparse jacobian dX/dp. + * The Jacobian is returned in jac as + * (dx_11/da_1, ..., dx_1m/da_m, ..., dx_n1/da_1, ..., dx_nm/da_m, + * dx_11/db_1, ..., dx_1m/db_1, ..., dx_n1/db_n, ..., dx_nm/db_n), or (using HZ's notation), + * jac=(A_11, B_11, ..., A_1m, B_1m, ..., A_n1, B_n1, ..., A_nm, B_nm) + * Notice that depending on idxij, some of the A_ij and B_ij might be missing. + * Note also that A_ij and B_ij are mnp x cnp and mnp x pnp matrices resp. and they + * should be stored in jac in row-major order. + * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used + * as working memory + * + * If NULL, the jacobian is approximated by repetitive func calls and finite + * differences. This is computationally inefficient and thus NOT recommended. + */ + void *adata, /* pointer to possibly additional data, passed uninterpreted to func, fjac */ + + const int itmax, /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */ + const int verbose, /* I: verbosity */ + const double opts[SBA_OPTSSZ], + /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial \mu, + * stopping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2 + */ + double info[SBA_INFOSZ] + /* O: information regarding the minimization. Set to NULL if don't care + * info[0]=||e||_2 at initial p. + * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. + * info[5]= # iterations, + * info[6]=reason for terminating: 1 - stopped by small gradient J^T e + * 2 - stopped by small dp + * 3 - stopped by itmax + * 4 - stopped by small relative reduction in ||e||_2 + * 5 - stopped by small ||e||_2 + * 6 - too many attempts to increase damping. Restart with increased mu + * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error + * info[7]= # function evaluations + * info[8]= # jacobian evaluations + * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error + */ +) +{ +register int i, j, ii, jj, k, l; +int nvis, nnz, retval; + +/* The following are work arrays that are dynamically allocated by sba_motstr_levmar_x() */ +double *jac; /* work array for storing the jacobian, max. size n*m*(mnp*cnp + mnp*pnp) */ +double *U; /* work array for storing the U_j in the order U_1, ..., U_m, size m*cnp*cnp */ +double *V; /* work array for storing the *strictly upper triangles* of V_i in the order V_1, ..., V_n, size n*pnp*pnp. + * V also stores the lower triangles of (V*_i)^-1 in the order (V*_1)^-1, ..., (V*_n)^-1. + * Note that diagonal elements of V_1 are saved in diagUV + */ + +double *e; /* work array for storing the e_ij in the order e_11, ..., e_1m, ..., e_n1, ..., e_nm, + max. size n*m*mnp */ +double *eab; /* work array for storing the ea_j & eb_i in the order ea_1, .. ea_m eb_1, .. eb_n size m*cnp + n*pnp */ + +double *E; /* work array for storing the e_j in the order e_1, .. e_m, size m*cnp */ + +/* Notice that the blocks W_ij, Y_ij are zero iff A_ij (equivalently B_ij) is zero. This means + * that the matrix consisting of blocks W_ij is itself sparse, similarly to the + * block matrix made up of the A_ij and B_ij (i.e. jac) + */ +double *W; /* work array for storing the W_ij in the order W_11, ..., W_1m, ..., W_n1, ..., W_nm, + max. size n*m*cnp*pnp */ +double *Yj; /* work array for storing the Y_ij for a *fixed* j in the order Y_1j, Y_nj, + max. size n*cnp*pnp */ +double *YWt; /* work array for storing \sum_i Y_ij W_ik^T, size cnp*cnp */ +double *S; /* work array for storing the block array S_jk, size m*m*cnp*cnp */ +double *dp; /* work array for storing the parameter vector updates da_1, ..., da_m, db_1, ..., db_n, size m*cnp + n*pnp */ +double *Wtda; /* work array for storing \sum_j W_ij^T da_j, size pnp */ +double *wght= /* work array for storing the weights computed from the covariance inverses, max. size n*m*mnp*mnp */ + NULL; + +/* Of the above arrays, jac, e, W, Yj, wght are sparse and + * U, V, eab, E, S, dp are dense. Sparse arrays (except Yj) are indexed + * through idxij (see below), that is with the same mechanism as the input + * measurements vector x + */ + +double *pa, *pb, *ea, *eb, *dpa, *dpb; /* pointers into p, jac, eab and dp respectively */ + +/* submatrices sizes */ +int Asz, Bsz, ABsz, Usz, Vsz, + Wsz, Ysz, esz, easz, ebsz, + YWtsz, Wtdasz, Sblsz, covsz; + +int Sdim; /* S matrix actual dimension */ + +register double *ptr1, *ptr2, *ptr3, *ptr4, sum; +struct sba_crsm idxij; /* sparse matrix containing the location of x_ij in x. This is also + * the location of A_ij, B_ij in jac, etc. + * This matrix can be thought as a map from a sparse set of pairs (i, j) to a continuous + * index k and it is used to efficiently lookup the memory locations where the non-zero + * blocks of a sparse matrix/vector are stored + */ +int maxCvis, /* max. of projections of a single point across cameras, <=m */ + maxPvis, /* max. of projections in a single camera across points, <=n */ + maxCPvis, /* max. of the above */ + *rcidxs, /* work array for the indexes corresponding to the nonzero elements of a single row or + column in a sparse matrix, size max(n, m) */ + *rcsubs; /* work array for the subscripts of nonzero elements in a single row or column of a + sparse matrix, size max(n, m) */ + +/* The following variables are needed by the LM algorithm */ +register int itno; /* iteration counter */ +int issolved; +/* temporary work arrays that are dynamically allocated */ +double *hx, /* \hat{x}_i, max. size m*n*mnp */ + *diagUV, /* diagonals of U_j, V_i, size m*cnp + n*pnp */ + *pdp; /* p + dp, size m*cnp + n*pnp */ + +double *diagU, *diagV; /* pointers into diagUV */ + +register double mu, /* damping constant */ + tmp; /* mainly used in matrix & vector multiplications */ +double p_eL2, eab_inf, pdp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+dp)||_2 */ +double p_L2, dp_L2=DBL_MAX, dF, dL; +double tau=FABS(opts[0]), eps1=FABS(opts[1]), eps2=FABS(opts[2]), eps2_sq=opts[2]*opts[2], + eps3_sq=opts[3]*opts[3], eps4_sq=opts[4]*opts[4]; +double init_p_eL2; +int nu=2, nu2, stop=0, nfev, njev=0, nlss=0; +int nobs, nvars; +const int mmcon=m-mcon; +PLS linsolver=NULL; +int (*matinv)(double *A, int m)=NULL; + +struct fdj_data_x_ fdj_data; +void *jac_adata; + +/* Initialization */ + mu=eab_inf=0.0; /* -Wall */ + + /* block sizes */ + Asz=mnp * cnp; Bsz=mnp * pnp; ABsz=Asz + Bsz; + Usz=cnp * cnp; Vsz=pnp * pnp; + Wsz=cnp * pnp; Ysz=cnp * pnp; + esz=mnp; + easz=cnp; ebsz=pnp; + YWtsz=cnp * cnp; + Wtdasz=pnp; + Sblsz=cnp * cnp; + Sdim=mmcon * cnp; + covsz=mnp * mnp; + + /* count total number of visible image points */ + for(i=nvis=0, jj=n*m; i<jj; ++i) + nvis+=(vmask[i]!=0); + + nobs=nvis*mnp; + nvars=m*cnp + n*pnp; + if(nobs<nvars){ + fprintf(stderr, "SBA: sba_motstr_levmar_x() cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n", nobs, nvars); + return SBA_ERROR; + } + + /* allocate & fill up the idxij structure */ + sba_crsm_alloc(&idxij, n, m, nvis); + for(i=k=0; i<n; ++i){ + idxij.rowptr[i]=k; + ii=i*m; + for(j=0; j<m; ++j) + if(vmask[ii+j]){ + idxij.val[k]=k; + idxij.colidx[k++]=j; + } + } + idxij.rowptr[n]=nvis; + + /* find the maximum number (for all cameras) of visible image projections coming from a single 3D point */ + for(i=maxCvis=0; i<n; ++i) + if((k=idxij.rowptr[i+1]-idxij.rowptr[i])>maxCvis) maxCvis=k; + + /* find the maximum number (for all points) of visible image projections in any single camera */ + for(j=maxPvis=0; j<m; ++j){ + for(i=ii=0; i<n; ++i) + if(vmask[i*m+j]) ++ii; + if(ii>maxPvis) maxPvis=ii; + } + maxCPvis=(maxCvis>=maxPvis)? maxCvis : maxPvis; + +#if 0 + /* determine the density of blocks in matrix S */ + for(j=mcon, ii=0; j<m; ++j){ + ++ii; /* block Sjj is surely nonzero */ + for(k=j+1; k<m; ++k) + if(sba_crsm_common_row(&idxij, j, k)) ii+=2; /* blocks Sjk & Skj are nonzero */ + } + printf("\nS block density: %.5g\n", ((double)ii)/(mmcon*mmcon)); fflush(stdout); +#endif + + /* allocate work arrays */ + /* W is big enough to hold both jac & W. Note also the extra Wsz, see the initialization of jac below for explanation */ + W=(double *)emalloc((nvis*((Wsz>=ABsz)? Wsz : ABsz) + Wsz)*sizeof(double)); + U=(double *)emalloc(m*Usz*sizeof(double)); + V=(double *)emalloc(n*Vsz*sizeof(double)); + e=(double *)emalloc(nobs*sizeof(double)); + eab=(double *)emalloc(nvars*sizeof(double)); + E=(double *)emalloc(m*cnp*sizeof(double)); + Yj=(double *)emalloc(maxPvis*Ysz*sizeof(double)); + YWt=(double *)emalloc(YWtsz*sizeof(double)); + S=(double *)emalloc(m*m*Sblsz*sizeof(double)); + dp=(double *)emalloc(nvars*sizeof(double)); + Wtda=(double *)emalloc(pnp*sizeof(double)); + rcidxs=(int *)emalloc(maxCPvis*sizeof(int)); + rcsubs=(int *)emalloc(maxCPvis*sizeof(int)); +#ifndef SBA_DESTROY_COVS + if(covx!=NULL) wght=(double *)emalloc(nvis*covsz*sizeof(double)); +#else + if(covx!=NULL) wght=covx; +#endif /* SBA_DESTROY_COVS */ + + + hx=(double *)emalloc(nobs*sizeof(double)); + diagUV=(double *)emalloc(nvars*sizeof(double)); + pdp=(double *)emalloc(nvars*sizeof(double)); + + /* to save resources, W and jac share the same memory: First, the jacobian + * is computed in some working memory that is then overwritten during the + * computation of W. To account for the case of W being larger than jac, + * extra memory is reserved "before" jac. + * Care must be taken, however, to ensure that storing a certain W_ij + * does not overwrite the A_ij, B_ij used to compute it. To achieve + * this is, note that if p1 and p2 respectively point to the first elements + * of a certain W_ij and A_ij, B_ij pair, we should have p2-p1>=Wsz. + * There are two cases: + * a) Wsz>=ABsz: Then p1=W+k*Wsz and p2=jac+k*ABsz=W+Wsz+nvis*(Wsz-ABsz)+k*ABsz + * for some k (0<=k<nvis), thus p2-p1=(nvis-k)*(Wsz-ABsz)+Wsz. + * The right side of the last equation is obviously > Wsz for all 0<=k<nvis + * + * b) Wsz<ABsz: Then p1=W+k*Wsz and p2=jac+k*ABsz=W+Wsz+k*ABsz and + * p2-p1=Wsz+k*(ABsz-Wsz), which is again > Wsz for all 0<=k<nvis + * + * In conclusion, if jac is initialized as below, the memory allocated to all + * W_ij is guaranteed not to overlap with that allocated to their corresponding + * A_ij, B_ij pairs + */ + jac=W + Wsz + ((Wsz>ABsz)? nvis*(Wsz-ABsz) : 0); + + /* set up auxiliary pointers */ + pa=p; pb=p+m*cnp; + ea=eab; eb=eab+m*cnp; + dpa=dp; dpb=dp+m*cnp; + + diagU=diagUV; diagV=diagUV + m*cnp; + + /* if no jacobian function is supplied, prepare to compute jacobian with finite difference */ + if(!fjac){ + fdj_data.func=func; + fdj_data.cnp=cnp; + fdj_data.pnp=pnp; + fdj_data.mnp=mnp; + fdj_data.hx=hx; + fdj_data.hxx=(double *)emalloc(nobs*sizeof(double)); + fdj_data.func_rcidxs=(int *)emalloc(2*maxCPvis*sizeof(int)); + fdj_data.func_rcsubs=fdj_data.func_rcidxs+maxCPvis; + fdj_data.adata=adata; + + fjac=sba_fdjac_x; + jac_adata=(void *)&fdj_data; + } + else{ + fdj_data.hxx=NULL; + jac_adata=adata; + } + + if(itmax==0){ /* verify jacobian */ + sba_motstr_chkjac_x(func, fjac, p, &idxij, rcidxs, rcsubs, ncon, mcon, cnp, pnp, mnp, adata, jac_adata); + retval=0; + goto freemem_and_return; + } + + /* covariances Sigma_x_ij are accommodated by computing the Cholesky decompositions of their + * inverses and using the resulting matrices w_x_ij to weigh A_ij, B_ij, and e_ij as w_x_ij A_ij, + * w_x_ij*B_ij and w_x_ij*e_ij. In this way, auxiliary variables as U_j=\sum_i A_ij^T A_ij + * actually become \sum_i (w_x_ij A_ij)^T w_x_ij A_ij= \sum_i A_ij^T w_x_ij^T w_x_ij A_ij = + * A_ij^T Sigma_x_ij^-1 A_ij + * + * ea_j, V_i, eb_i, W_ij are weighted in a similar manner + */ + if(covx!=NULL){ + for(i=0; i<n; ++i){ + nnz=sba_crsm_row_elmidxs(&idxij, i, rcidxs, rcsubs); /* find nonzero x_ij, j=0...m-1 */ + for(j=0; j<nnz; ++j){ + /* set ptr1, ptr2 to point to cov_x_ij, w_x_ij resp. */ + ptr1=covx + (k=idxij.val[rcidxs[j]]*covsz); + ptr2=wght + k; + if(!sba_mat_cholinv(ptr1, ptr2, mnp)){ /* compute w_x_ij s.t. w_x_ij^T w_x_ij = cov_x_ij^-1 */ + fprintf(stderr, "SBA: invalid covariance matrix for x_ij (i=%d, j=%d) in sba_motstr_levmar_x()\n", i, rcsubs[j]); + retval=SBA_ERROR; + goto freemem_and_return; + } + } + } + sba_mat_cholinv(NULL, NULL, 0); /* cleanup */ + } + + /* compute the error vectors e_ij in hx */ + (*func)(p, &idxij, rcidxs, rcsubs, hx, adata); nfev=1; + /* ### compute e=x - f(p) [e=w*(x - f(p)] and its L2 norm */ + if(covx==NULL) + p_eL2=nrmL2xmy(e, x, hx, nobs); /* e=x-hx, p_eL2=||e|| */ + else + p_eL2=nrmCxmy(e, x, hx, wght, mnp, nvis); /* e=wght*(x-hx), p_eL2=||e||=||x-hx||_Sigma^-1 */ + if(verbose) printf("initial motstr-SBA error %g [%g]\n", p_eL2, p_eL2/nvis); + init_p_eL2=p_eL2; + if(!SBA_FINITE(p_eL2)) stop=7; + + for(itno=0; itno<itmax && !stop; ++itno){ + /* Note that p, e and ||e||_2 have been updated at the previous iteration */ + + /* compute derivative submatrices A_ij, B_ij */ + (*fjac)(p, &idxij, rcidxs, rcsubs, jac, jac_adata); ++njev; + + if(covx!=NULL){ + /* compute w_x_ij A_ij and w_x_ij B_ij. + * Since w_x_ij is upper triangular, the products can be safely saved + * directly in A_ij, B_ij, without the need for intermediate storage + */ + for(i=0; i<nvis; ++i){ + /* set ptr1, ptr2, ptr3 to point to w_x_ij, A_ij, B_ij, resp. */ + ptr1=wght + i*covsz; + ptr2=jac + i*ABsz; + ptr3=ptr2 + Asz; // ptr3=jac + i*ABsz + Asz; + + /* w_x_ij is mnp x mnp, A_ij is mnp x cnp, B_ij is mnp x pnp + * NOTE: Jamming the outter (i.e., ii) loops did not run faster! + */ + /* A_ij */ + for(ii=0; ii<mnp; ++ii) + for(jj=0; jj<cnp; ++jj){ + for(k=ii, sum=0.0; k<mnp; ++k) // k>=ii since w_x_ij is upper triangular + sum+=ptr1[ii*mnp+k]*ptr2[k*cnp+jj]; + ptr2[ii*cnp+jj]=sum; + } + + /* B_ij */ + for(ii=0; ii<mnp; ++ii) + for(jj=0; jj<pnp; ++jj){ + for(k=ii, sum=0.0; k<mnp; ++k) // k>=ii since w_x_ij is upper triangular + sum+=ptr1[ii*mnp+k]*ptr3[k*pnp+jj]; + ptr3[ii*pnp+jj]=sum; + } + } + } + + /* compute U_j = \sum_i A_ij^T A_ij */ // \Sigma here! + /* U_j is symmetric, therefore its computation can be sped up by + * computing only the upper part and then reusing it for the lower one. + * Recall that A_ij is mnp x cnp + */ + /* Also compute ea_j = \sum_i A_ij^T e_ij */ // \Sigma here! + /* Recall that e_ij is mnp x 1 + */ + _dblzero(U, m*Usz); /* clear all U_j */ + _dblzero(ea, m*easz); /* clear all ea_j */ + for(j=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + ptr2=ea + j*easz; // set ptr2 to point to ea_j + + nnz=sba_crsm_col_elmidxs(&idxij, j, rcidxs, rcsubs); /* find nonzero A_ij, i=0...n-1 */ + for(i=0; i<nnz; ++i){ + /* set ptr3 to point to A_ij, actual row number in rcsubs[i] */ + ptr3=jac + idxij.val[rcidxs[i]]*ABsz; + + /* compute the UPPER TRIANGULAR PART of A_ij^T A_ij and add it to U_j */ + for(ii=0; ii<cnp; ++ii){ + for(jj=ii; jj<cnp; ++jj){ + for(k=0, sum=0.0; k<mnp; ++k) + sum+=ptr3[k*cnp+ii]*ptr3[k*cnp+jj]; + ptr1[ii*cnp+jj]+=sum; + } + + /* copy the LOWER TRIANGULAR PART of U_j from the upper one */ + for(jj=0; jj<ii; ++jj) + ptr1[ii*cnp+jj]=ptr1[jj*cnp+ii]; + } + + ptr4=e + idxij.val[rcidxs[i]]*esz; /* set ptr4 to point to e_ij */ + /* compute A_ij^T e_ij and add it to ea_j */ + for(ii=0; ii<cnp; ++ii){ + for(jj=0, sum=0.0; jj<mnp; ++jj) + sum+=ptr3[jj*cnp+ii]*ptr4[jj]; + ptr2[ii]+=sum; + } + } + } + + /* compute V_i = \sum_j B_ij^T B_ij */ // \Sigma here! + /* V_i is symmetric, therefore its computation can be sped up by + * computing only the upper part and then reusing it for the lower one. + * Recall that B_ij is mnp x pnp + */ + /* Also compute eb_i = \sum_j B_ij^T e_ij */ // \Sigma here! + /* Recall that e_ij is mnp x 1 + */ + _dblzero(V, n*Vsz); /* clear all V_i */ + _dblzero(eb, n*ebsz); /* clear all eb_i */ + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + ptr2=eb + i*ebsz; // set ptr2 to point to eb_i + + nnz=sba_crsm_row_elmidxs(&idxij, i, rcidxs, rcsubs); /* find nonzero B_ij, j=0...m-1 */ + for(j=0; j<nnz; ++j){ + /* set ptr3 to point to B_ij, actual column number in rcsubs[j] */ + ptr3=jac + idxij.val[rcidxs[j]]*ABsz + Asz; + + /* compute the UPPER TRIANGULAR PART of B_ij^T B_ij and add it to V_i */ + for(ii=0; ii<pnp; ++ii){ + for(jj=ii; jj<pnp; ++jj){ + for(k=0, sum=0.0; k<mnp; ++k) + sum+=ptr3[k*pnp+ii]*ptr3[k*pnp+jj]; + ptr1[ii*pnp+jj]+=sum; + } + } + + ptr4=e + idxij.val[rcidxs[j]]*esz; /* set ptr4 to point to e_ij */ + /* compute B_ij^T e_ij and add it to eb_i */ + for(ii=0; ii<pnp; ++ii){ + for(jj=0, sum=0.0; jj<mnp; ++jj) + sum+=ptr3[jj*pnp+ii]*ptr4[jj]; + ptr2[ii]+=sum; + } + } + } + + /* compute W_ij = A_ij^T B_ij */ // \Sigma here! + /* Recall that A_ij is mnp x cnp and B_ij is mnp x pnp + */ + for(i=ncon; i<n; ++i){ + nnz=sba_crsm_row_elmidxs(&idxij, i, rcidxs, rcsubs); /* find nonzero W_ij, j=0...m-1 */ + for(j=0; j<nnz; ++j){ + /* set ptr1 to point to W_ij, actual column number in rcsubs[j] */ + ptr1=W + idxij.val[rcidxs[j]]*Wsz; + + if(rcsubs[j]<mcon){ /* A_ij is zero */ + //_dblzero(ptr1, Wsz); /* clear W_ij */ + continue; + } + + /* set ptr2 & ptr3 to point to A_ij & B_ij resp. */ + ptr2=jac + idxij.val[rcidxs[j]]*ABsz; + ptr3=ptr2 + Asz; + /* compute A_ij^T B_ij and store it in W_ij + * Recall that storage for A_ij, B_ij does not overlap with that for W_ij, + * see the comments related to the initialization of jac above + */ + /* assert(ptr2-ptr1>=Wsz); */ + for(ii=0; ii<cnp; ++ii) + for(jj=0; jj<pnp; ++jj){ + for(k=0, sum=0.0; k<mnp; ++k) + sum+=ptr2[k*cnp+ii]*ptr3[k*pnp+jj]; + ptr1[ii*pnp+jj]=sum; + } + } + } + + /* Compute ||J^T e||_inf and ||p||^2 */ + for(i=0, p_L2=eab_inf=0.0; i<nvars; ++i){ + if(eab_inf < (tmp=FABS(eab[i]))) eab_inf=tmp; + p_L2+=p[i]*p[i]; + } + //p_L2=sqrt(p_L2); + + /* save diagonal entries so that augmentation can be later canceled. + * Diagonal entries are in U_j and V_i + */ + for(j=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + ptr2=diagU + j*cnp; // set ptr2 to point to diagU_j + for(i=0; i<cnp; ++i) + ptr2[i]=ptr1[i*cnp+i]; + } + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + ptr2=diagV + i*pnp; // set ptr2 to point to diagV_i + for(j=0; j<pnp; ++j) + ptr2[j]=ptr1[j*pnp+j]; + } + +/* +if(!(itno%100)){ + printf("Current estimate: "); + for(i=0; i<nvars; ++i) + printf("%.9g ", p[i]); + printf("-- errors %.9g %0.9g\n", eab_inf, p_eL2); +} +*/ + + /* check for convergence */ + if((eab_inf <= eps1)){ + dp_L2=0.0; /* no increment for p in this case */ + stop=1; + break; + } + + /* compute initial damping factor */ + if(itno==0){ + /* find max diagonal element */ + for(i=mcon*cnp, tmp=DBL_MIN; i<m*cnp; ++i) + if(diagUV[i]>tmp) tmp=diagUV[i]; + for(i=m*cnp + ncon*pnp; i<nvars; ++i) /* tmp is not re-initialized! */ + if(diagUV[i]>tmp) tmp=diagUV[i]; + mu=tau*tmp; + } + + /* determine increment using adaptive damping */ + while(1){ + /* augment U, V */ + for(j=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + for(i=0; i<cnp; ++i) + ptr1[i*cnp+i]+=mu; + } + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + for(j=0; j<pnp; ++j) + ptr1[j*pnp+j]+=mu; + + /* compute V*_i^-1. + * Recall that only the upper triangle of the symmetric pnp x pnp matrix V*_i + * is stored in ptr1; its (also symmetric) inverse is saved in the lower triangle of ptr1 + */ + /* inverting V*_i with LDLT seems to result in faster overall execution compared to when using LU or Cholesky */ + //j=sba_symat_invert_LU(ptr1, pnp); matinv=sba_symat_invert_LU; + //j=sba_symat_invert_Chol(ptr1, pnp); matinv=sba_symat_invert_Chol; + j=sba_symat_invert_BK(ptr1, pnp); matinv=sba_symat_invert_BK; + if(!j){ + fprintf(stderr, "SBA: singular matrix V*_i (i=%d) in sba_motstr_levmar_x(), increasing damping\n", i); + goto moredamping; // increasing damping will eventually make V*_i diagonally dominant, thus nonsingular + //retval=SBA_ERROR; + //goto freemem_and_return; + } + } + + _dblzero(E, m*easz); /* clear all e_j */ + /* compute the mmcon x mmcon block matrix S and e_j */ + + /* Note that S is symmetric, therefore its computation can be + * sped up by computing only the upper part and then reusing + * it for the lower one. + */ + for(j=mcon; j<m; ++j){ + int mmconxUsz=mmcon*Usz; + + nnz=sba_crsm_col_elmidxs(&idxij, j, rcidxs, rcsubs); /* find nonzero Y_ij, i=0...n-1 */ + + /* get rid of all Y_ij with i<ncon that are treated as zeros. + * In this way, all rcsubs[i] below are guaranteed to be >= ncon + */ + if(ncon){ + for(i=ii=0; i<nnz; ++i){ + if(rcsubs[i]>=ncon){ + rcidxs[ii]=rcidxs[i]; + rcsubs[ii++]=rcsubs[i]; + } + } + nnz=ii; + } + + /* compute all Y_ij = W_ij (V*_i)^-1 for a *fixed* j. + * To save memory, the block matrix consisting of the Y_ij + * is not stored. Instead, only a block column of this matrix + * is computed & used at each time: For each j, all nonzero + * Y_ij are computed in Yj and then used in the calculations + * involving S_jk and e_j. + * Recall that W_ij is cnp x pnp and (V*_i) is pnp x pnp + */ + for(i=0; i<nnz; ++i){ + /* set ptr3 to point to (V*_i)^-1, actual row number in rcsubs[i] */ + ptr3=V + rcsubs[i]*Vsz; + + /* set ptr1 to point to Y_ij, actual row number in rcsubs[i] */ + ptr1=Yj + i*Ysz; + /* set ptr2 to point to W_ij resp. */ + ptr2=W + idxij.val[rcidxs[i]]*Wsz; + /* compute W_ij (V*_i)^-1 and store it in Y_ij. + * Recall that only the lower triangle of (V*_i)^-1 is stored + */ + for(ii=0; ii<cnp; ++ii){ + ptr4=ptr2+ii*pnp; + for(jj=0; jj<pnp; ++jj){ + for(k=0, sum=0.0; k<=jj; ++k) + sum+=ptr4[k]*ptr3[jj*pnp+k]; //ptr2[ii*pnp+k]*ptr3[jj*pnp+k]; + for( ; k<pnp; ++k) + sum+=ptr4[k]*ptr3[k*pnp+jj]; //ptr2[ii*pnp+k]*ptr3[k*pnp+jj]; + ptr1[ii*pnp+jj]=sum; + } + } + } + + /* compute the UPPER TRIANGULAR PART of S */ + for(k=j; k<m; ++k){ // j>=mcon + /* compute \sum_i Y_ij W_ik^T in YWt. Note that for an off-diagonal block defined by j, k + * YWt (and thus S_jk) is nonzero only if there exists a point that is visible in both the + * j-th and k-th images + */ + + /* Recall that Y_ij is cnp x pnp and W_ik is cnp x pnp */ + _dblzero(YWt, YWtsz); /* clear YWt */ + + for(i=0; i<nnz; ++i){ + register double *pYWt; + + /* find the min and max column indices of the elements in row i (actually rcsubs[i]) + * and make sure that k falls within them. This test handles W_ik's which are + * certain to be zero without bothering to call sba_crsm_elmidx() + */ + ii=idxij.colidx[idxij.rowptr[rcsubs[i]]]; + jj=idxij.colidx[idxij.rowptr[rcsubs[i]+1]-1]; + if(k<ii || k>jj) continue; /* W_ik == 0 */ + + /* set ptr2 to point to W_ik */ + l=sba_crsm_elmidxp(&idxij, rcsubs[i], k, j, rcidxs[i]); + //l=sba_crsm_elmidx(&idxij, rcsubs[i], k); + if(l==-1) continue; /* W_ik == 0 */ + + ptr2=W + idxij.val[l]*Wsz; + /* set ptr1 to point to Y_ij, actual row number in rcsubs[i] */ + ptr1=Yj + i*Ysz; + for(ii=0; ii<cnp; ++ii){ + ptr3=ptr1+ii*pnp; + pYWt=YWt+ii*cnp; + + for(jj=0; jj<cnp; ++jj){ + ptr4=ptr2+jj*pnp; + for(l=0, sum=0.0; l<pnp; ++l) + sum+=ptr3[l]*ptr4[l]; //ptr1[ii*pnp+l]*ptr2[jj*pnp+l]; + pYWt[jj]+=sum; //YWt[ii*cnp+jj]+=sum; + } + } + } + + /* since the linear system involving S is solved with lapack, + * it is preferable to store S in column major (i.e. fortran) + * order, so as to avoid unecessary transposing/copying. + */ +#if MAT_STORAGE==COLUMN_MAJOR + ptr2=S + (k-mcon)*mmconxUsz + (j-mcon)*cnp; // set ptr2 to point to the beginning of block j,k in S +#else + ptr2=S + (j-mcon)*mmconxUsz + (k-mcon)*cnp; // set ptr2 to point to the beginning of block j,k in S +#endif + + if(j!=k){ /* Kronecker */ + for(ii=0; ii<cnp; ++ii, ptr2+=Sdim) + for(jj=0; jj<cnp; ++jj) + ptr2[jj]= +#if MAT_STORAGE==COLUMN_MAJOR + -YWt[jj*cnp+ii]; +#else + -YWt[ii*cnp+jj]; +#endif + } + else{ + ptr1=U + j*Usz; // set ptr1 to point to U_j + + for(ii=0; ii<cnp; ++ii, ptr2+=Sdim) + for(jj=0; jj<cnp; ++jj) + ptr2[jj]= +#if MAT_STORAGE==COLUMN_MAJOR + ptr1[jj*cnp+ii] - YWt[jj*cnp+ii]; +#else + ptr1[ii*cnp+jj] - YWt[ii*cnp+jj]; +#endif + } + } + + /* copy the LOWER TRIANGULAR PART of S from the upper one */ + for(k=mcon; k<j; ++k){ +#if MAT_STORAGE==COLUMN_MAJOR + ptr1=S + (k-mcon)*mmconxUsz + (j-mcon)*cnp; // set ptr1 to point to the beginning of block j,k in S + ptr2=S + (j-mcon)*mmconxUsz + (k-mcon)*cnp; // set ptr2 to point to the beginning of block k,j in S +#else + ptr1=S + (j-mcon)*mmconxUsz + (k-mcon)*cnp; // set ptr1 to point to the beginning of block j,k in S + ptr2=S + (k-mcon)*mmconxUsz + (j-mcon)*cnp; // set ptr2 to point to the beginning of block k,j in S +#endif + for(ii=0; ii<cnp; ++ii, ptr1+=Sdim) + for(jj=0, ptr3=ptr2+ii; jj<cnp; ++jj, ptr3+=Sdim) + ptr1[jj]=*ptr3; + } + + /* compute e_j=ea_j - \sum_i Y_ij eb_i */ + /* Recall that Y_ij is cnp x pnp and eb_i is pnp x 1 */ + ptr1=E + j*easz; // set ptr1 to point to e_j + + for(i=0; i<nnz; ++i){ + /* set ptr2 to point to Y_ij, actual row number in rcsubs[i] */ + ptr2=Yj + i*Ysz; + + /* set ptr3 to point to eb_i */ + ptr3=eb + rcsubs[i]*ebsz; + for(ii=0; ii<cnp; ++ii){ + ptr4=ptr2+ii*pnp; + for(jj=0, sum=0.0; jj<pnp; ++jj) + sum+=ptr4[jj]*ptr3[jj]; //ptr2[ii*pnp+jj]*ptr3[jj]; + ptr1[ii]+=sum; + } + } + + ptr2=ea + j*easz; // set ptr2 to point to ea_j + for(i=0; i<easz; ++i) + ptr1[i]=ptr2[i] - ptr1[i]; + } + +#if 0 + if(verbose>1){ /* count the nonzeros in S */ + for(i=ii=0; i<Sdim*Sdim; ++i) + if(S[i]!=0.0) ++ii; + printf("\nS density: %.5g\n", ((double)ii)/(Sdim*Sdim)); fflush(stdout); + } +#endif + + /* solve the linear system S dpa = E to compute the da_j. + * + * Note that if MAT_STORAGE==1 S is modified in the following call; + * this is OK since S is recomputed for each iteration + */ + //issolved=sba_Axb_LU(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_LU; + issolved=sba_Axb_Chol(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_Chol; + //issolved=sba_Axb_BK(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_BK; + //issolved=sba_Axb_QRnoQ(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_QRnoQ; + //issolved=sba_Axb_QR(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_QR; + //issolved=sba_Axb_SVD(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, MAT_STORAGE); linsolver=sba_Axb_SVD; + //issolved=sba_Axb_CG(S, E+mcon*cnp, dpa+mcon*cnp, Sdim, (3*Sdim)/2, 1E-10, SBA_CG_JACOBI, MAT_STORAGE); linsolver=(PLS)sba_Axb_CG; + + ++nlss; + + _dblzero(dpa, mcon*cnp); /* no change for the first mcon camera params */ + + if(issolved){ + + /* compute the db_i */ + for(i=ncon; i<n; ++i){ + ptr1=dpb + i*ebsz; // set ptr1 to point to db_i + + /* compute \sum_j W_ij^T da_j */ + /* Recall that W_ij is cnp x pnp and da_j is cnp x 1 */ + _dblzero(Wtda, Wtdasz); /* clear Wtda */ + nnz=sba_crsm_row_elmidxs(&idxij, i, rcidxs, rcsubs); /* find nonzero W_ij, j=0...m-1 */ + for(j=0; j<nnz; ++j){ + /* set ptr2 to point to W_ij, actual column number in rcsubs[j] */ + if(rcsubs[j]<mcon) continue; /* W_ij is zero */ + + ptr2=W + idxij.val[rcidxs[j]]*Wsz; + + /* set ptr3 to point to da_j */ + ptr3=dpa + rcsubs[j]*cnp; + + for(ii=0; ii<pnp; ++ii){ + ptr4=ptr2+ii; + for(jj=0, sum=0.0; jj<cnp; ++jj) + sum+=ptr4[jj*pnp]*ptr3[jj]; //ptr2[jj*pnp+ii]*ptr3[jj]; + Wtda[ii]+=sum; + } + } + + /* compute eb_i - \sum_j W_ij^T da_j = eb_i - Wtda in Wtda */ + ptr2=eb + i*ebsz; // set ptr2 to point to eb_i + for(ii=0; ii<pnp; ++ii) + Wtda[ii]=ptr2[ii] - Wtda[ii]; + + /* compute the product (V*_i)^-1 Wtda = (V*_i)^-1 (eb_i - \sum_j W_ij^T da_j). + * Recall that only the lower triangle of (V*_i)^-1 is stored + */ + ptr2=V + i*Vsz; // set ptr2 to point to (V*_i)^-1 + for(ii=0; ii<pnp; ++ii){ + for(jj=0, sum=0.0; jj<=ii; ++jj) + sum+=ptr2[ii*pnp+jj]*Wtda[jj]; + for( ; jj<pnp; ++jj) + sum+=ptr2[jj*pnp+ii]*Wtda[jj]; + ptr1[ii]=sum; + } + } + _dblzero(dpb, ncon*pnp); /* no change for the first ncon point params */ + + /* parameter vector updates are now in dpa, dpb */ + + /* compute p's new estimate and ||dp||^2 */ + for(i=0, dp_L2=0.0; i<nvars; ++i){ + pdp[i]=p[i] + (tmp=dp[i]); + dp_L2+=tmp*tmp; + } + //dp_L2=sqrt(dp_L2); + + if(dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */ + //if(dp_L2<=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */ + stop=2; + break; + } + + if(dp_L2>=(p_L2+eps2)/SBA_EPSILON_SQ){ /* almost singular */ + //if(dp_L2>=(p_L2+eps2)/SBA_EPSILON){ /* almost singular */ + fprintf(stderr, "SBA: the matrix of the augmented normal equations is almost singular in sba_motstr_levmar_x(),\n" + " minimization should be restarted from the current solution with an increased damping term\n"); + retval=SBA_ERROR; + goto freemem_and_return; + } + + (*func)(pdp, &idxij, rcidxs, rcsubs, hx, adata); ++nfev; /* evaluate function at p + dp */ + if(verbose>1) + printf("mean reprojection error %g\n", sba_mean_repr_error(n, mnp, x, hx, &idxij, rcidxs, rcsubs)); + /* ### compute ||e(pdp)||_2 */ + if(covx==NULL) + pdp_eL2=nrmL2xmy(hx, x, hx, nobs); /* hx=x-hx, pdp_eL2=||hx|| */ + else + pdp_eL2=nrmCxmy(hx, x, hx, wght, mnp, nvis); /* hx=wght*(x-hx), pdp_eL2=||hx|| */ + if(!SBA_FINITE(pdp_eL2)){ + if(verbose) /* identify the offending point projection */ + sba_print_inf(hx, m, mnp, &idxij, rcidxs, rcsubs); + + stop=7; + break; + } + + for(i=0, dL=0.0; i<nvars; ++i) + dL+=dp[i]*(mu*dp[i]+eab[i]); + + dF=p_eL2-pdp_eL2; + + if(verbose>1) + printf("\ndamping term %8g, gain ratio %8g, errors %8g / %8g = %g\n", mu, dL!=0.0? dF/dL : dF/DBL_EPSILON, p_eL2/nvis, pdp_eL2/nvis, p_eL2/pdp_eL2); + + if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */ + tmp=(2.0*dF/dL-1.0); + tmp=1.0-tmp*tmp*tmp; + mu=mu*( (tmp>=SBA_ONE_THIRD)? tmp : SBA_ONE_THIRD ); + nu=2; + + /* the test below is equivalent to the relative reduction of the RMS reprojection error: sqrt(p_eL2)-sqrt(pdp_eL2)<eps4*sqrt(p_eL2) */ + if(pdp_eL2-2.0*sqrt(p_eL2*pdp_eL2)<(eps4_sq-1.0)*p_eL2) stop=4; + + for(i=0; i<nvars; ++i) /* update p's estimate */ + p[i]=pdp[i]; + + for(i=0; i<nobs; ++i) /* update e and ||e||_2 */ + e[i]=hx[i]; + p_eL2=pdp_eL2; + break; + } + } /* issolved */ + +moredamping: + /* if this point is reached (also via an explicit goto!), either the linear system could + * not be solved or the error did not reduce; in any case, the increment must be rejected + */ + + mu*=nu; + nu2=nu<<1; // 2*nu; + if(nu2<=nu){ /* nu has wrapped around (overflown) */ + fprintf(stderr, "SBA: too many failed attempts to increase the damping factor in sba_motstr_levmar_x()! Singular Hessian matrix?\n"); + //exit(1); + stop=6; + break; + } + nu=nu2; + +#if 0 + /* restore U, V diagonal entries */ + for(j=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + ptr2=diagU + j*cnp; // set ptr2 to point to diagU_j + for(i=0; i<cnp; ++i) + ptr1[i*cnp+i]=ptr2[i]; + } + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + ptr2=diagV + i*pnp; // set ptr2 to point to diagV_i + for(j=0; j<pnp; ++j) + ptr1[j*pnp+j]=ptr2[j]; + } +#endif + } /* inner while loop */ + + if(p_eL2<=eps3_sq) stop=5; // error is small, force termination of outer loop + } + + if(itno>=itmax) stop=3; + + /* restore U, V diagonal entries */ + for(j=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + ptr2=diagU + j*cnp; // set ptr2 to point to diagU_j + for(i=0; i<cnp; ++i) + ptr1[i*cnp+i]=ptr2[i]; + } + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + ptr2=diagV + i*pnp; // set ptr2 to point to diagV_i + for(j=0; j<pnp; ++j) + ptr1[j*pnp+j]=ptr2[j]; + } + + if(info){ + info[0]=init_p_eL2; + info[1]=p_eL2; + info[2]=eab_inf; + info[3]=dp_L2; + for(j=mcon, tmp=DBL_MIN; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + for(i=0; i<cnp; ++i) + if(tmp<ptr1[i*cnp+i]) tmp=ptr1[i*cnp+i]; + } + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + for(j=0; j<pnp; ++j) + if(tmp<ptr1[j*pnp+j]) tmp=ptr1[j*pnp+j]; + } + info[4]=mu/tmp; + info[5]=itno; + info[6]=stop; + info[7]=nfev; + info[8]=njev; + info[9]=nlss; + } + + //sba_print_sol(n, m, p, cnp, pnp, x, mnp, &idxij, rcidxs, rcsubs); + retval=(stop!=7)? itno : SBA_ERROR; + +freemem_and_return: /* NOTE: this point is also reached via a goto! */ + + /* free whatever was allocated */ + free(W); free(U); free(V); + free(e); free(eab); + free(E); free(Yj); free(YWt); + free(S); free(dp); free(Wtda); + free(rcidxs); free(rcsubs); +#ifndef SBA_DESTROY_COVS + if(wght) free(wght); +#else + /* nothing to do */ +#endif /* SBA_DESTROY_COVS */ + + free(hx); free(diagUV); free(pdp); + if(fdj_data.hxx){ // cleanup + free(fdj_data.hxx); + free(fdj_data.func_rcidxs); + } + + sba_crsm_free(&idxij); + + /* free the memory allocated by the matrix inversion & linear solver routines */ + if(matinv) (*matinv)(NULL, 0); + if(linsolver) (*linsolver)(NULL, NULL, NULL, 0, 0); + + return retval; +} + + +/* Bundle adjustment on camera parameters only + * using the sparse Levenberg-Marquardt as described in HZ p. 568 + * + * Returns the number of iterations (>=0) if successfull, SBA_ERROR if failed + */ + +int sba_mot_levmar_x( + const int n, /* number of points */ + const int m, /* number of images */ + const int mcon,/* number of images (starting from the 1st) whose parameters should not be modified. + * All A_ij (see below) with j<mcon are assumed to be zero + */ + char *vmask, /* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */ + double *p, /* initial parameter vector p0: (a1, ..., am). + * aj are the image j parameters, size m*cnp */ + const int cnp,/* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */ + double *x, /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where + * x_ij is the projection of the i-th point on the j-th image. + * NOTE: some of the x_ij might be missing, if point i is not visible in image j; + * see vmask[i, j], max. size n*m*mnp + */ + double *covx, /* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm), + * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no + * covariance estimates are available (identity matrices are implicitly used in this case). + * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing; + * see vmask[i, j], max. size n*m*mnp*mnp + */ + const int mnp,/* number of parameters for EACH measurement; usually 2 */ + void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata), + /* functional relation describing measurements. Given a parameter vector p, + * computes a prediction of the measurements \hat{x}. p is (m*cnp)x1, + * \hat{x} is (n*m*mnp)x1, maximum + * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used + * as working memory + */ + void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata), + /* function to evaluate the sparse jacobian dX/dp. + * The Jacobian is returned in jac as + * (dx_11/da_1, ..., dx_1m/da_m, ..., dx_n1/da_1, ..., dx_nm/da_m), or (using HZ's notation), + * jac=(A_11, ..., A_1m, ..., A_n1, ..., A_nm) + * Notice that depending on idxij, some of the A_ij might be missing. + * Note also that the A_ij are mnp x cnp matrices and they + * should be stored in jac in row-major order. + * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used + * as working memory + * + * If NULL, the jacobian is approximated by repetitive func calls and finite + * differences. This is computationally inefficient and thus NOT recommended. + */ + void *adata, /* pointer to possibly additional data, passed uninterpreted to func, fjac */ + + const int itmax, /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */ + const int verbose, /* I: verbosity */ + const double opts[SBA_OPTSSZ], + /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial \mu, + * stopping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2 + */ + double info[SBA_INFOSZ] + /* O: information regarding the minimization. Set to NULL if don't care + * info[0]=||e||_2 at initial p. + * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. + * info[5]= # iterations, + * info[6]=reason for terminating: 1 - stopped by small gradient J^T e + * 2 - stopped by small dp + * 3 - stopped by itmax + * 4 - stopped by small relative reduction in ||e||_2 + * 5 - stopped by small ||e||_2 + * 6 - too many attempts to increase damping. Restart with increased mu + * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error + * info[7]= # function evaluations + * info[8]= # jacobian evaluations + * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error + */ +) +{ +register int i, j, ii, jj, k; +int nvis, nnz, retval; + +/* The following are work arrays that are dynamically allocated by sba_mot_levmar_x() */ +double *jac; /* work array for storing the jacobian, max. size n*m*mnp*cnp */ +double *U; /* work array for storing the U_j in the order U_1, ..., U_m, size m*cnp*cnp */ + +double *e; /* work array for storing the e_ij in the order e_11, ..., e_1m, ..., e_n1, ..., e_nm, + max. size n*m*mnp */ +double *ea; /* work array for storing the ea_j in the order ea_1, .. ea_m, size m*cnp */ + +double *dp; /* work array for storing the parameter vector updates da_1, ..., da_m, size m*cnp */ + +double *wght= /* work array for storing the weights computed from the covariance inverses, max. size n*m*mnp*mnp */ + NULL; + +/* Of the above arrays, jac, e, wght are sparse and + * U, ea, dp are dense. Sparse arrays are indexed through + * idxij (see below), that is with the same mechanism as the input + * measurements vector x + */ + +/* submatrices sizes */ +int Asz, Usz, + esz, easz, covsz; + +register double *ptr1, *ptr2, *ptr3, *ptr4, sum; +struct sba_crsm idxij; /* sparse matrix containing the location of x_ij in x. This is also the location of A_ij + * in jac, e_ij in e, etc. + * This matrix can be thought as a map from a sparse set of pairs (i, j) to a continuous + * index k and it is used to efficiently lookup the memory locations where the non-zero + * blocks of a sparse matrix/vector are stored + */ +int maxCPvis, /* max. of projections across cameras & projections across points */ + *rcidxs, /* work array for the indexes corresponding to the nonzero elements of a single row or + column in a sparse matrix, size max(n, m) */ + *rcsubs; /* work array for the subscripts of nonzero elements in a single row or column of a + sparse matrix, size max(n, m) */ + +/* The following variables are needed by the LM algorithm */ +register int itno; /* iteration counter */ +int nsolved; +/* temporary work arrays that are dynamically allocated */ +double *hx, /* \hat{x}_i, max. size m*n*mnp */ + *diagU, /* diagonals of U_j, size m*cnp */ + *pdp; /* p + dp, size m*cnp */ + +register double mu, /* damping constant */ + tmp; /* mainly used in matrix & vector multiplications */ +double p_eL2, ea_inf, pdp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+dp)||_2 */ +double p_L2, dp_L2=DBL_MAX, dF, dL; +double tau=FABS(opts[0]), eps1=FABS(opts[1]), eps2=FABS(opts[2]), eps2_sq=opts[2]*opts[2], + eps3_sq=opts[3]*opts[3], eps4_sq=opts[4]*opts[4]; +double init_p_eL2; +int nu=2, nu2, stop=0, nfev, njev=0, nlss=0; +int nobs, nvars; +PLS linsolver=NULL; + +struct fdj_data_x_ fdj_data; +void *jac_adata; + +/* Initialization */ + mu=ea_inf=0.0; /* -Wall */ + + /* block sizes */ + Asz=mnp * cnp; Usz=cnp * cnp; + esz=mnp; easz=cnp; + covsz=mnp * mnp; + + /* count total number of visible image points */ + for(i=nvis=0, jj=n*m; i<jj; ++i) + nvis+=(vmask[i]!=0); + + nobs=nvis*mnp; + nvars=m*cnp; + if(nobs<nvars){ + fprintf(stderr, "SBA: sba_mot_levmar_x() cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n", nobs, nvars); + return SBA_ERROR; + } + + /* allocate & fill up the idxij structure */ + sba_crsm_alloc(&idxij, n, m, nvis); + for(i=k=0; i<n; ++i){ + idxij.rowptr[i]=k; + ii=i*m; + for(j=0; j<m; ++j) + if(vmask[ii+j]){ + idxij.val[k]=k; + idxij.colidx[k++]=j; + } + } + idxij.rowptr[n]=nvis; + + /* find the maximum number of visible image points in any single camera or coming from a single 3D point */ + /* cameras */ + for(i=maxCPvis=0; i<n; ++i) + if((k=idxij.rowptr[i+1]-idxij.rowptr[i])>maxCPvis) maxCPvis=k; + + /* points, note that maxCPvis is not reinitialized! */ + for(j=0; j<m; ++j){ + for(i=ii=0; i<n; ++i) + if(vmask[i*m+j]) ++ii; + if(ii>maxCPvis) maxCPvis=ii; + } + + /* allocate work arrays */ + jac=(double *)emalloc(nvis*Asz*sizeof(double)); + U=(double *)emalloc(m*Usz*sizeof(double)); + e=(double *)emalloc(nobs*sizeof(double)); + ea=(double *)emalloc(nvars*sizeof(double)); + dp=(double *)emalloc(nvars*sizeof(double)); + rcidxs=(int *)emalloc(maxCPvis*sizeof(int)); + rcsubs=(int *)emalloc(maxCPvis*sizeof(int)); +#ifndef SBA_DESTROY_COVS + if(covx!=NULL) wght=(double *)emalloc(nvis*covsz*sizeof(double)); +#else + if(covx!=NULL) wght=covx; +#endif /* SBA_DESTROY_COVS */ + + + hx=(double *)emalloc(nobs*sizeof(double)); + diagU=(double *)emalloc(nvars*sizeof(double)); + pdp=(double *)emalloc(nvars*sizeof(double)); + + /* if no jacobian function is supplied, prepare to compute jacobian with finite difference */ + if(!fjac){ + fdj_data.func=func; + fdj_data.cnp=cnp; + fdj_data.pnp=0; + fdj_data.mnp=mnp; + fdj_data.hx=hx; + fdj_data.hxx=(double *)emalloc(nobs*sizeof(double)); + fdj_data.func_rcidxs=(int *)emalloc(2*maxCPvis*sizeof(int)); + fdj_data.func_rcsubs=fdj_data.func_rcidxs+maxCPvis; + fdj_data.adata=adata; + + fjac=sba_fdjac_x; + jac_adata=(void *)&fdj_data; + } + else{ + fdj_data.hxx=NULL; + jac_adata=adata; + } + + if(itmax==0){ /* verify jacobian */ + sba_mot_chkjac_x(func, fjac, p, &idxij, rcidxs, rcsubs, mcon, cnp, mnp, adata, jac_adata); + retval=0; + goto freemem_and_return; + } + + /* covariances Sigma_x_ij are accommodated by computing the Cholesky decompositions of their + * inverses and using the resulting matrices w_x_ij to weigh A_ij and e_ij as w_x_ij A_ij + * and w_x_ij*e_ij. In this way, auxiliary variables as U_j=\sum_i A_ij^T A_ij + * actually become \sum_i (w_x_ij A_ij)^T w_x_ij A_ij= \sum_i A_ij^T w_x_ij^T w_x_ij A_ij = + * A_ij^T Sigma_x_ij^-1 A_ij + * + * ea_j are weighted in a similar manner + */ + if(covx!=NULL){ + for(i=0; i<n; ++i){ + nnz=sba_crsm_row_elmidxs(&idxij, i, rcidxs, rcsubs); /* find nonzero x_ij, j=0...m-1 */ + for(j=0; j<nnz; ++j){ + /* set ptr1, ptr2 to point to cov_x_ij, w_x_ij resp. */ + ptr1=covx + (k=idxij.val[rcidxs[j]]*covsz); + ptr2=wght + k; + if(!sba_mat_cholinv(ptr1, ptr2, mnp)){ /* compute w_x_ij s.t. w_x_ij^T w_x_ij = cov_x_ij^-1 */ + fprintf(stderr, "SBA: invalid covariance matrix for x_ij (i=%d, j=%d) in sba_motstr_levmar_x()\n", i, rcsubs[j]); + retval=SBA_ERROR; + goto freemem_and_return; + } + } + } + sba_mat_cholinv(NULL, NULL, 0); /* cleanup */ + } + + /* compute the error vectors e_ij in hx */ + (*func)(p, &idxij, rcidxs, rcsubs, hx, adata); nfev=1; + /* ### compute e=x - f(p) [e=w*(x - f(p)] and its L2 norm */ + if(covx==NULL) + p_eL2=nrmL2xmy(e, x, hx, nobs); /* e=x-hx, p_eL2=||e|| */ + else + p_eL2=nrmCxmy(e, x, hx, wght, mnp, nvis); /* e=wght*(x-hx), p_eL2=||e||=||x-hx||_Sigma^-1 */ + if(verbose) printf("initial mot-SBA error %g [%g]\n", p_eL2, p_eL2/nvis); + init_p_eL2=p_eL2; + if(!SBA_FINITE(p_eL2)) stop=7; + + for(itno=0; itno<itmax && !stop; ++itno){ + /* Note that p, e and ||e||_2 have been updated at the previous iteration */ + + /* compute derivative submatrices A_ij */ + (*fjac)(p, &idxij, rcidxs, rcsubs, jac, jac_adata); ++njev; + + if(covx!=NULL){ + /* compute w_x_ij A_ij + * Since w_x_ij is upper triangular, the products can be safely saved + * directly in A_ij, without the need for intermediate storage + */ + for(i=0; i<nvis; ++i){ + /* set ptr1, ptr2 to point to w_x_ij, A_ij, resp. */ + ptr1=wght + i*covsz; + ptr2=jac + i*Asz; + + /* w_x_ij is mnp x mnp, A_ij is mnp x cnp */ + for(ii=0; ii<mnp; ++ii) + for(jj=0; jj<cnp; ++jj){ + for(k=ii, sum=0.0; k<mnp; ++k) // k>=ii since w_x_ij is upper triangular + sum+=ptr1[ii*mnp+k]*ptr2[k*cnp+jj]; + ptr2[ii*cnp+jj]=sum; + } + } + } + + /* compute U_j = \sum_i A_ij^T A_ij */ // \Sigma here! + /* U_j is symmetric, therefore its computation can be sped up by + * computing only the upper part and then reusing it for the lower one. + * Recall that A_ij is mnp x cnp + */ + /* Also compute ea_j = \sum_i A_ij^T e_ij */ // \Sigma here! + /* Recall that e_ij is mnp x 1 + */ + _dblzero(U, m*Usz); /* clear all U_j */ + _dblzero(ea, m*easz); /* clear all ea_j */ + for(j=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + ptr2=ea + j*easz; // set ptr2 to point to ea_j + + nnz=sba_crsm_col_elmidxs(&idxij, j, rcidxs, rcsubs); /* find nonzero A_ij, i=0...n-1 */ + for(i=0; i<nnz; ++i){ + /* set ptr3 to point to A_ij, actual row number in rcsubs[i] */ + ptr3=jac + idxij.val[rcidxs[i]]*Asz; + + /* compute the UPPER TRIANGULAR PART of A_ij^T A_ij and add it to U_j */ + for(ii=0; ii<cnp; ++ii){ + for(jj=ii; jj<cnp; ++jj){ + for(k=0, sum=0.0; k<mnp; ++k) + sum+=ptr3[k*cnp+ii]*ptr3[k*cnp+jj]; + ptr1[ii*cnp+jj]+=sum; + } + + /* copy the LOWER TRIANGULAR PART of U_j from the upper one */ + for(jj=0; jj<ii; ++jj) + ptr1[ii*cnp+jj]=ptr1[jj*cnp+ii]; + } + + ptr4=e + idxij.val[rcidxs[i]]*esz; /* set ptr4 to point to e_ij */ + /* compute A_ij^T e_ij and add it to ea_j */ + for(ii=0; ii<cnp; ++ii){ + for(jj=0, sum=0.0; jj<mnp; ++jj) + sum+=ptr3[jj*cnp+ii]*ptr4[jj]; + ptr2[ii]+=sum; + } + } + } + + /* Compute ||J^T e||_inf and ||p||^2 */ + for(i=0, p_L2=ea_inf=0.0; i<nvars; ++i){ + if(ea_inf < (tmp=FABS(ea[i]))) ea_inf=tmp; + p_L2+=p[i]*p[i]; + } + //p_L2=sqrt(p_L2); + + /* save diagonal entries so that augmentation can be later canceled. + * Diagonal entries are in U_j + */ + for(j=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + ptr2=diagU + j*cnp; // set ptr2 to point to diagU_j + for(i=0; i<cnp; ++i) + ptr2[i]=ptr1[i*cnp+i]; + } + +/* +if(!(itno%100)){ + printf("Current estimate: "); + for(i=0; i<nvars; ++i) + printf("%.9g ", p[i]); + printf("-- errors %.9g %0.9g\n", ea_inf, p_eL2); +} +*/ + + /* check for convergence */ + if((ea_inf <= eps1)){ + dp_L2=0.0; /* no increment for p in this case */ + stop=1; + break; + } + + /* compute initial damping factor */ + if(itno==0){ + for(i=mcon*cnp, tmp=DBL_MIN; i<nvars; ++i) + if(diagU[i]>tmp) tmp=diagU[i]; /* find max diagonal element */ + mu=tau*tmp; + } + + /* determine increment using adaptive damping */ + while(1){ + /* augment U */ + for(j=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + for(i=0; i<cnp; ++i) + ptr1[i*cnp+i]+=mu; + } + + /* solve the linear systems U_j da_j = ea_j to compute the da_j */ + _dblzero(dp, mcon*cnp); /* no change for the first mcon camera params */ + for(j=nsolved=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + ptr2=ea + j*easz; // set ptr2 to point to ea_j + ptr3=dp + j*cnp; // set ptr3 to point to da_j + + //nsolved+=sba_Axb_LU(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_LU; + nsolved+=sba_Axb_Chol(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_Chol; + //nsolved+=sba_Axb_BK(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_BK; + //nsolved+=sba_Axb_QRnoQ(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_QRnoQ; + //nsolved+=sba_Axb_QR(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_QR; + //nsolved+=sba_Axb_SVD(ptr1, ptr2, ptr3, cnp, 0); linsolver=sba_Axb_SVD; + //nsolved+=(sba_Axb_CG(ptr1, ptr2, ptr3, cnp, (3*cnp)/2, 1E-10, SBA_CG_JACOBI, 0) > 0); linsolver=(PLS)sba_Axb_CG; + + ++nlss; + } + + if(nsolved==m){ + + /* parameter vector updates are now in dp */ + + /* compute p's new estimate and ||dp||^2 */ + for(i=0, dp_L2=0.0; i<nvars; ++i){ + pdp[i]=p[i] + (tmp=dp[i]); + dp_L2+=tmp*tmp; + } + //dp_L2=sqrt(dp_L2); + + if(dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */ + //if(dp_L2<=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */ + stop=2; + break; + } + + if(dp_L2>=(p_L2+eps2)/SBA_EPSILON_SQ){ /* almost singular */ + //if(dp_L2>=(p_L2+eps2)/SBA_EPSILON){ /* almost singular */ + fprintf(stderr, "SBA: the matrix of the augmented normal equations is almost singular in sba_mot_levmar_x(),\n" + " minimization should be restarted from the current solution with an increased damping term\n"); + retval=SBA_ERROR; + goto freemem_and_return; + } + + (*func)(pdp, &idxij, rcidxs, rcsubs, hx, adata); ++nfev; /* evaluate function at p + dp */ + if(verbose>1) + printf("mean reprojection error %g\n", sba_mean_repr_error(n, mnp, x, hx, &idxij, rcidxs, rcsubs)); + /* ### compute ||e(pdp)||_2 */ + if(covx==NULL) + pdp_eL2=nrmL2xmy(hx, x, hx, nobs); /* hx=x-hx, pdp_eL2=||hx|| */ + else + pdp_eL2=nrmCxmy(hx, x, hx, wght, mnp, nvis); /* hx=wght*(x-hx), pdp_eL2=||hx|| */ + if(!SBA_FINITE(pdp_eL2)){ + if(verbose) /* identify the offending point projection */ + sba_print_inf(hx, m, mnp, &idxij, rcidxs, rcsubs); + + stop=7; + break; + } + + for(i=0, dL=0.0; i<nvars; ++i) + dL+=dp[i]*(mu*dp[i]+ea[i]); + + dF=p_eL2-pdp_eL2; + + if(verbose>1) + printf("\ndamping term %8g, gain ratio %8g, errors %8g / %8g = %g\n", mu, dL!=0.0? dF/dL : dF/DBL_EPSILON, p_eL2/nvis, pdp_eL2/nvis, p_eL2/pdp_eL2); + + if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */ + tmp=(2.0*dF/dL-1.0); + tmp=1.0-tmp*tmp*tmp; + mu=mu*( (tmp>=SBA_ONE_THIRD)? tmp : SBA_ONE_THIRD ); + nu=2; + + /* the test below is equivalent to the relative reduction of the RMS reprojection error: sqrt(p_eL2)-sqrt(pdp_eL2)<eps4*sqrt(p_eL2) */ + if(pdp_eL2-2.0*sqrt(p_eL2*pdp_eL2)<(eps4_sq-1.0)*p_eL2) stop=4; + + for(i=0; i<nvars; ++i) /* update p's estimate */ + p[i]=pdp[i]; + + for(i=0; i<nobs; ++i) /* update e and ||e||_2 */ + e[i]=hx[i]; + p_eL2=pdp_eL2; + break; + } + } /* nsolved==m */ + + /* if this point is reached, either at least one linear system could not be solved or + * the error did not reduce; in any case, the increment must be rejected + */ + + mu*=nu; + nu2=nu<<1; // 2*nu; + if(nu2<=nu){ /* nu has wrapped around (overflown) */ + fprintf(stderr, "SBA: too many failed attempts to increase the damping factor in sba_mot_levmar_x()! Singular Hessian matrix?\n"); + //exit(1); + stop=6; + break; + } + nu=nu2; + +#if 0 + /* restore U diagonal entries */ + for(j=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + ptr2=diagU + j*cnp; // set ptr2 to point to diagU_j + for(i=0; i<cnp; ++i) + ptr1[i*cnp+i]=ptr2[i]; + } +#endif + } /* inner while loop */ + + if(p_eL2<=eps3_sq) stop=5; // error is small, force termination of outer loop + } + + if(itno>=itmax) stop=3; + + /* restore U diagonal entries */ + for(j=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + ptr2=diagU + j*cnp; // set ptr2 to point to diagU_j + for(i=0; i<cnp; ++i) + ptr1[i*cnp+i]=ptr2[i]; + } + + if(info){ + info[0]=init_p_eL2; + info[1]=p_eL2; + info[2]=ea_inf; + info[3]=dp_L2; + for(j=mcon, tmp=DBL_MIN; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + for(i=0; i<cnp; ++i) + if(tmp<ptr1[i*cnp+i]) tmp=ptr1[i*cnp+i]; + } + info[4]=mu/tmp; + info[5]=itno; + info[6]=stop; + info[7]=nfev; + info[8]=njev; + info[9]=nlss; + } + //sba_print_sol(n, m, p, cnp, 0, x, mnp, &idxij, rcidxs, rcsubs); + retval=(stop!=7)? itno : SBA_ERROR; + +freemem_and_return: /* NOTE: this point is also reached via a goto! */ + + /* free whatever was allocated */ + free(jac); free(U); + free(e); free(ea); + free(dp); + free(rcidxs); free(rcsubs); +#ifndef SBA_DESTROY_COVS + if(wght) free(wght); +#else + /* nothing to do */ +#endif /* SBA_DESTROY_COVS */ + + free(hx); free(diagU); free(pdp); + if(fdj_data.hxx){ // cleanup + free(fdj_data.hxx); + free(fdj_data.func_rcidxs); + } + + sba_crsm_free(&idxij); + + /* free the memory allocated by the linear solver routine */ + if(linsolver) (*linsolver)(NULL, NULL, NULL, 0, 0); + + return retval; +} + + +/* Bundle adjustment on structure parameters only + * using the sparse Levenberg-Marquardt as described in HZ p. 568 + * + * Returns the number of iterations (>=0) if successfull, SBA_ERROR if failed + */ + +int sba_str_levmar_x( + const int n, /* number of points */ + const int ncon,/* number of points (starting from the 1st) whose parameters should not be modified. + * All B_ij (see below) with i<ncon are assumed to be zero + */ + const int m, /* number of images */ + char *vmask, /* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */ + double *p, /* initial parameter vector p0: (b1, ..., bn). + * bi are the i-th point parameters, * size n*pnp */ + const int pnp,/* number of parameters for ONE point; e.g. 3 for Euclidean points */ + double *x, /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where + * x_ij is the projection of the i-th point on the j-th image. + * NOTE: some of the x_ij might be missing, if point i is not visible in image j; + * see vmask[i, j], max. size n*m*mnp + */ + double *covx, /* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm), + * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no + * covariance estimates are available (identity matrices are implicitly used in this case). + * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing; + * see vmask[i, j], max. size n*m*mnp*mnp + */ + const int mnp,/* number of parameters for EACH measurement; usually 2 */ + void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata), + /* functional relation describing measurements. Given a parameter vector p, + * computes a prediction of the measurements \hat{x}. p is (n*pnp)x1, + * \hat{x} is (n*m*mnp)x1, maximum + * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used + * as working memory + */ + void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata), + /* function to evaluate the sparse jacobian dX/dp. + * The Jacobian is returned in jac as + * (dx_11/db_1, ..., dx_1m/db_1, ..., dx_n1/db_n, ..., dx_nm/db_n), or (using HZ's notation), + * jac=(B_11, ..., B_1m, ..., B_n1, ..., B_nm) + * Notice that depending on idxij, some of the B_ij might be missing. + * Note also that B_ij are mnp x pnp matrices and they + * should be stored in jac in row-major order. + * rcidxs, rcsubs are max(m, n) x 1, allocated by the caller and can be used + * as working memory + * + * If NULL, the jacobian is approximated by repetitive func calls and finite + * differences. This is computationally inefficient and thus NOT recommended. + */ + void *adata, /* pointer to possibly additional data, passed uninterpreted to func, fjac */ + + const int itmax, /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */ + const int verbose, /* I: verbosity */ + const double opts[SBA_OPTSSZ], + /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial \mu, + * stopping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2 + */ + double info[SBA_INFOSZ] + /* O: information regarding the minimization. Set to NULL if don't care + * info[0]=||e||_2 at initial p. + * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. + * info[5]= # iterations, + * info[6]=reason for terminating: 1 - stopped by small gradient J^T e + * 2 - stopped by small dp + * 3 - stopped by itmax + * 4 - stopped by small relative reduction in ||e||_2 + * 5 - stopped by small ||e||_2 + * 6 - too many attempts to increase damping. Restart with increased mu + * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error + * info[7]= # function evaluations + * info[8]= # jacobian evaluations + * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error + */ +) +{ +register int i, j, ii, jj, k; +int nvis, nnz, retval; + +/* The following are work arrays that are dynamically allocated by sba_str_levmar_x() */ +double *jac; /* work array for storing the jacobian, max. size n*m*mnp*pnp */ +double *V; /* work array for storing the V_i in the order V_1, ..., V_n, size n*pnp*pnp */ + +double *e; /* work array for storing the e_ij in the order e_11, ..., e_1m, ..., e_n1, ..., e_nm, + max. size n*m*mnp */ +double *eb; /* work array for storing the eb_i in the order eb_1, .. eb_n size n*pnp */ + +double *dp; /* work array for storing the parameter vector updates db_1, ..., db_n, size n*pnp */ + +double *wght= /* work array for storing the weights computed from the covariance inverses, max. size n*m*mnp*mnp */ + NULL; + +/* Of the above arrays, jac, e, wght are sparse and + * V, eb, dp are dense. Sparse arrays are indexed through + * idxij (see below), that is with the same mechanism as the input + * measurements vector x + */ + +/* submatrices sizes */ +int Bsz, Vsz, + esz, ebsz, covsz; + +register double *ptr1, *ptr2, *ptr3, *ptr4, sum; +struct sba_crsm idxij; /* sparse matrix containing the location of x_ij in x. This is also the location + * of B_ij in jac, etc. + * This matrix can be thought as a map from a sparse set of pairs (i, j) to a continuous + * index k and it is used to efficiently lookup the memory locations where the non-zero + * blocks of a sparse matrix/vector are stored + */ +int maxCPvis, /* max. of projections across cameras & projections across points */ + *rcidxs, /* work array for the indexes corresponding to the nonzero elements of a single row or + column in a sparse matrix, size max(n, m) */ + *rcsubs; /* work array for the subscripts of nonzero elements in a single row or column of a + sparse matrix, size max(n, m) */ + +/* The following variables are needed by the LM algorithm */ +register int itno; /* iteration counter */ +int nsolved; +/* temporary work arrays that are dynamically allocated */ +double *hx, /* \hat{x}_i, max. size m*n*mnp */ + *diagV, /* diagonals of V_i, size n*pnp */ + *pdp; /* p + dp, size n*pnp */ + +register double mu, /* damping constant */ + tmp; /* mainly used in matrix & vector multiplications */ +double p_eL2, eb_inf, pdp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+dp)||_2 */ +double p_L2, dp_L2=DBL_MAX, dF, dL; +double tau=FABS(opts[0]), eps1=FABS(opts[1]), eps2=FABS(opts[2]), eps2_sq=opts[2]*opts[2], + eps3_sq=opts[3]*opts[3], eps4_sq=opts[4]*opts[4]; +double init_p_eL2; +int nu=2, nu2, stop=0, nfev, njev=0, nlss=0; +int nobs, nvars; +PLS linsolver=NULL; + +struct fdj_data_x_ fdj_data; +void *jac_adata; + +/* Initialization */ + mu=eb_inf=tmp=0.0; /* -Wall */ + + /* block sizes */ + Bsz=mnp * pnp; Vsz=pnp * pnp; + esz=mnp; ebsz=pnp; + covsz=mnp * mnp; + + /* count total number of visible image points */ + for(i=nvis=0, jj=n*m; i<jj; ++i) + nvis+=(vmask[i]!=0); + + nobs=nvis*mnp; + nvars=n*pnp; + if(nobs<nvars){ + fprintf(stderr, "SBA: sba_str_levmar_x() cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n", nobs, nvars); + return SBA_ERROR; + } + + /* allocate & fill up the idxij structure */ + sba_crsm_alloc(&idxij, n, m, nvis); + for(i=k=0; i<n; ++i){ + idxij.rowptr[i]=k; + ii=i*m; + for(j=0; j<m; ++j) + if(vmask[ii+j]){ + idxij.val[k]=k; + idxij.colidx[k++]=j; + } + } + idxij.rowptr[n]=nvis; + + /* find the maximum number of visible image points in any single camera or coming from a single 3D point */ + /* cameras */ + for(i=maxCPvis=0; i<n; ++i) + if((k=idxij.rowptr[i+1]-idxij.rowptr[i])>maxCPvis) maxCPvis=k; + + /* points, note that maxCPvis is not reinitialized! */ + for(j=0; j<m; ++j){ + for(i=ii=0; i<n; ++i) + if(vmask[i*m+j]) ++ii; + if(ii>maxCPvis) maxCPvis=ii; + } + + /* allocate work arrays */ + jac=(double *)emalloc(nvis*Bsz*sizeof(double)); + V=(double *)emalloc(n*Vsz*sizeof(double)); + e=(double *)emalloc(nobs*sizeof(double)); + eb=(double *)emalloc(nvars*sizeof(double)); + dp=(double *)emalloc(nvars*sizeof(double)); + rcidxs=(int *)emalloc(maxCPvis*sizeof(int)); + rcsubs=(int *)emalloc(maxCPvis*sizeof(int)); +#ifndef SBA_DESTROY_COVS + if(covx!=NULL) wght=(double *)emalloc(nvis*covsz*sizeof(double)); +#else + if(covx!=NULL) wght=covx; +#endif /* SBA_DESTROY_COVS */ + + + hx=(double *)emalloc(nobs*sizeof(double)); + diagV=(double *)emalloc(nvars*sizeof(double)); + pdp=(double *)emalloc(nvars*sizeof(double)); + + /* if no jacobian function is supplied, prepare to compute jacobian with finite difference */ + if(!fjac){ + fdj_data.func=func; + fdj_data.cnp=0; + fdj_data.pnp=pnp; + fdj_data.mnp=mnp; + fdj_data.hx=hx; + fdj_data.hxx=(double *)emalloc(nobs*sizeof(double)); + fdj_data.func_rcidxs=(int *)emalloc(2*maxCPvis*sizeof(int)); + fdj_data.func_rcsubs=fdj_data.func_rcidxs+maxCPvis; + fdj_data.adata=adata; + + fjac=sba_fdjac_x; + jac_adata=(void *)&fdj_data; + } + else{ + fdj_data.hxx=NULL; + jac_adata=adata; + } + + if(itmax==0){ /* verify jacobian */ + sba_str_chkjac_x(func, fjac, p, &idxij, rcidxs, rcsubs, ncon, pnp, mnp, adata, jac_adata); + retval=0; + goto freemem_and_return; + } + + /* covariances Sigma_x_ij are accommodated by computing the Cholesky decompositions of their + * inverses and using the resulting matrices w_x_ij to weigh B_ij and e_ij as + * w_x_ij*B_ij and w_x_ij*e_ij. In this way, auxiliary variables as V_i=\sum_j B_ij^T B_ij + * actually become \sum_j (w_x_ij B_ij)^T w_x_ij B_ij= \sum_j B_ij^T w_x_ij^T w_x_ij B_ij = + * B_ij^T Sigma_x_ij^-1 B_ij + * + * eb_i are weighted in a similar manner + */ + if(covx!=NULL){ + for(i=0; i<n; ++i){ + nnz=sba_crsm_row_elmidxs(&idxij, i, rcidxs, rcsubs); /* find nonzero x_ij, j=0...m-1 */ + for(j=0; j<nnz; ++j){ + /* set ptr1, ptr2 to point to cov_x_ij, w_x_ij resp. */ + ptr1=covx + (k=idxij.val[rcidxs[j]]*covsz); + ptr2=wght + k; + if(!sba_mat_cholinv(ptr1, ptr2, mnp)){ /* compute w_x_ij s.t. w_x_ij^T w_x_ij = cov_x_ij^-1 */ + fprintf(stderr, "SBA: invalid covariance matrix for x_ij (i=%d, j=%d) in sba_motstr_levmar_x()\n", i, rcsubs[j]); + retval=SBA_ERROR; + goto freemem_and_return; + } + } + } + sba_mat_cholinv(NULL, NULL, 0); /* cleanup */ + } + + /* compute the error vectors e_ij in hx */ + (*func)(p, &idxij, rcidxs, rcsubs, hx, adata); nfev=1; + /* ### compute e=x - f(p) [e=w*(x - f(p)] and its L2 norm */ + if(covx==NULL) + p_eL2=nrmL2xmy(e, x, hx, nobs); /* e=x-hx, p_eL2=||e|| */ + else + p_eL2=nrmCxmy(e, x, hx, wght, mnp, nvis); /* e=wght*(x-hx), p_eL2=||e||=||x-hx||_Sigma^-1 */ + if(verbose) printf("initial str-SBA error %g [%g]\n", p_eL2, p_eL2/nvis); + init_p_eL2=p_eL2; + if(!SBA_FINITE(p_eL2)) stop=7; + + for(itno=0; itno<itmax && !stop; ++itno){ + /* Note that p, e and ||e||_2 have been updated at the previous iteration */ + + /* compute derivative submatrices B_ij */ + (*fjac)(p, &idxij, rcidxs, rcsubs, jac, jac_adata); ++njev; + + if(covx!=NULL){ + /* compute w_x_ij B_ij. + * Since w_x_ij is upper triangular, the products can be safely saved + * directly in B_ij, without the need for intermediate storage + */ + for(i=0; i<nvis; ++i){ + /* set ptr1, ptr2 to point to w_x_ij, B_ij, resp. */ + ptr1=wght + i*covsz; + ptr2=jac + i*Bsz; + + /* w_x_ij is mnp x mnp, B_ij is mnp x pnp */ + for(ii=0; ii<mnp; ++ii) + for(jj=0; jj<pnp; ++jj){ + for(k=ii, sum=0.0; k<mnp; ++k) // k>=ii since w_x_ij is upper triangular + sum+=ptr1[ii*mnp+k]*ptr2[k*pnp+jj]; + ptr2[ii*pnp+jj]=sum; + } + } + } + + /* compute V_i = \sum_j B_ij^T B_ij */ // \Sigma here! + /* V_i is symmetric, therefore its computation can be sped up by + * computing only the upper part and then reusing it for the lower one. + * Recall that B_ij is mnp x pnp + */ + /* Also compute eb_i = \sum_j B_ij^T e_ij */ // \Sigma here! + /* Recall that e_ij is mnp x 1 + */ + _dblzero(V, n*Vsz); /* clear all V_i */ + _dblzero(eb, n*ebsz); /* clear all eb_i */ + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + ptr2=eb + i*ebsz; // set ptr2 to point to eb_i + + nnz=sba_crsm_row_elmidxs(&idxij, i, rcidxs, rcsubs); /* find nonzero B_ij, j=0...m-1 */ + for(j=0; j<nnz; ++j){ + /* set ptr3 to point to B_ij, actual column number in rcsubs[j] */ + ptr3=jac + idxij.val[rcidxs[j]]*Bsz; + + /* compute the UPPER TRIANGULAR PART of B_ij^T B_ij and add it to V_i */ + for(ii=0; ii<pnp; ++ii){ + for(jj=ii; jj<pnp; ++jj){ + for(k=0, sum=0.0; k<mnp; ++k) + sum+=ptr3[k*pnp+ii]*ptr3[k*pnp+jj]; + ptr1[ii*pnp+jj]+=sum; + } + + /* copy the LOWER TRIANGULAR PART of V_i from the upper one */ + for(jj=0; jj<ii; ++jj) + ptr1[ii*pnp+jj]=ptr1[jj*pnp+ii]; + } + + ptr4=e + idxij.val[rcidxs[j]]*esz; /* set ptr4 to point to e_ij */ + /* compute B_ij^T e_ij and add it to eb_i */ + for(ii=0; ii<pnp; ++ii){ + for(jj=0, sum=0.0; jj<mnp; ++jj) + sum+=ptr3[jj*pnp+ii]*ptr4[jj]; + ptr2[ii]+=sum; + } + } + } + + /* Compute ||J^T e||_inf and ||p||^2 */ + for(i=0, p_L2=eb_inf=0.0; i<nvars; ++i){ + if(eb_inf < (tmp=FABS(eb[i]))) eb_inf=tmp; + p_L2+=p[i]*p[i]; + } + //p_L2=sqrt(p_L2); + + /* save diagonal entries so that augmentation can be later canceled. + * Diagonal entries are in V_i + */ + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + ptr2=diagV + i*pnp; // set ptr2 to point to diagV_i + for(j=0; j<pnp; ++j) + ptr2[j]=ptr1[j*pnp+j]; + } + +/* +if(!(itno%100)){ + printf("Current estimate: "); + for(i=0; i<nvars; ++i) + printf("%.9g ", p[i]); + printf("-- errors %.9g %0.9g\n", eb_inf, p_eL2); +} +*/ + + /* check for convergence */ + if((eb_inf <= eps1)){ + dp_L2=0.0; /* no increment for p in this case */ + stop=1; + break; + } + + /* compute initial damping factor */ + if(itno==0){ + for(i=ncon*pnp, tmp=DBL_MIN; i<nvars; ++i) + if(diagV[i]>tmp) tmp=diagV[i]; /* find max diagonal element */ + mu=tau*tmp; + } + + /* determine increment using adaptive damping */ + while(1){ + /* augment V */ + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + for(j=0; j<pnp; ++j) + ptr1[j*pnp+j]+=mu; + } + + /* solve the linear systems V*_i db_i = eb_i to compute the db_i */ + _dblzero(dp, ncon*pnp); /* no change for the first ncon point params */ + for(i=nsolved=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + ptr2=eb + i*ebsz; // set ptr2 to point to eb_i + ptr3=dp + i*pnp; // set ptr3 to point to db_i + + //nsolved+=sba_Axb_LU(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_LU; + nsolved+=sba_Axb_Chol(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_Chol; + //nsolved+=sba_Axb_BK(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_BK; + //nsolved+=sba_Axb_QRnoQ(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_QRnoQ; + //nsolved+=sba_Axb_QR(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_QR; + //nsolved+=sba_Axb_SVD(ptr1, ptr2, ptr3, pnp, 0); linsolver=sba_Axb_SVD; + //nsolved+=(sba_Axb_CG(ptr1, ptr2, ptr3, pnp, (3*pnp)/2, 1E-10, SBA_CG_JACOBI, 0) > 0); linsolver=(PLS)sba_Axb_CG; + + ++nlss; + } + + if(nsolved==n){ + + /* parameter vector updates are now in dp */ + + /* compute p's new estimate and ||dp||^2 */ + for(i=0, dp_L2=0.0; i<nvars; ++i){ + pdp[i]=p[i] + (tmp=dp[i]); + dp_L2+=tmp*tmp; + } + //dp_L2=sqrt(dp_L2); + + if(dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */ + //if(dp_L2<=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */ + stop=2; + break; + } + + if(dp_L2>=(p_L2+eps2)/SBA_EPSILON_SQ){ /* almost singular */ + //if(dp_L2>=(p_L2+eps2)/SBA_EPSILON){ /* almost singular */ + fprintf(stderr, "SBA: the matrix of the augmented normal equations is almost singular in sba_motstr_levmar_x(),\n" + " minimization should be restarted from the current solution with an increased damping term\n"); + retval=SBA_ERROR; + goto freemem_and_return; + } + + (*func)(pdp, &idxij, rcidxs, rcsubs, hx, adata); ++nfev; /* evaluate function at p + dp */ + if(verbose>1) + printf("mean reprojection error %g\n", sba_mean_repr_error(n, mnp, x, hx, &idxij, rcidxs, rcsubs)); + /* ### compute ||e(pdp)||_2 */ + if(covx==NULL) + pdp_eL2=nrmL2xmy(hx, x, hx, nobs); /* hx=x-hx, pdp_eL2=||hx|| */ + else + pdp_eL2=nrmCxmy(hx, x, hx, wght, mnp, nvis); /* hx=wght*(x-hx), pdp_eL2=||hx|| */ + if(!SBA_FINITE(pdp_eL2)){ + if(verbose) /* identify the offending point projection */ + sba_print_inf(hx, m, mnp, &idxij, rcidxs, rcsubs); + + stop=7; + break; + } + + for(i=0, dL=0.0; i<nvars; ++i) + dL+=dp[i]*(mu*dp[i]+eb[i]); + + dF=p_eL2-pdp_eL2; + + if(verbose>1) + printf("\ndamping term %8g, gain ratio %8g, errors %8g / %8g = %g\n", mu, dL!=0.0? dF/dL : dF/DBL_EPSILON, p_eL2/nvis, pdp_eL2/nvis, p_eL2/pdp_eL2); + + if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */ + tmp=(2.0*dF/dL-1.0); + tmp=1.0-tmp*tmp*tmp; + mu=mu*( (tmp>=SBA_ONE_THIRD)? tmp : SBA_ONE_THIRD ); + nu=2; + + /* the test below is equivalent to the relative reduction of the RMS reprojection error: sqrt(p_eL2)-sqrt(pdp_eL2)<eps4*sqrt(p_eL2) */ + if(pdp_eL2-2.0*sqrt(p_eL2*pdp_eL2)<(eps4_sq-1.0)*p_eL2) stop=4; + + for(i=0; i<nvars; ++i) /* update p's estimate */ + p[i]=pdp[i]; + + for(i=0; i<nobs; ++i) /* update e and ||e||_2 */ + e[i]=hx[i]; + p_eL2=pdp_eL2; + break; + } + } /* nsolved==n */ + + /* if this point is reached, either at least one linear system could not be solved or + * the error did not reduce; in any case, the increment must be rejected + */ + + mu*=nu; + nu2=nu<<1; // 2*nu; + if(nu2<=nu){ /* nu has wrapped around (overflown) */ + fprintf(stderr, "SBA: too many failed attempts to increase the damping factor in sba_str_levmar_x()! Singular Hessian matrix?\n"); + //exit(1); + stop=6; + break; + } + nu=nu2; + +#if 0 + /* restore V diagonal entries */ + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + ptr2=diagV + i*pnp; // set ptr2 to point to diagV_i + for(j=0; j<pnp; ++j) + ptr1[j*pnp+j]=ptr2[j]; + } +#endif + } /* inner while loop */ + + if(p_eL2<=eps3_sq) stop=5; // error is small, force termination of outer loop + } + + if(itno>=itmax) stop=3; + + /* restore V diagonal entries */ + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + ptr2=diagV + i*pnp; // set ptr2 to point to diagV_i + for(j=0; j<pnp; ++j) + ptr1[j*pnp+j]=ptr2[j]; + } + + if(info){ + info[0]=init_p_eL2; + info[1]=p_eL2; + info[2]=eb_inf; + info[3]=dp_L2; + for(i=ncon; i<n; ++i){ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + for(j=0; j<pnp; ++j) + if(tmp<ptr1[j*pnp+j]) tmp=ptr1[j*pnp+j]; + } + info[4]=mu/tmp; + info[5]=itno; + info[6]=stop; + info[7]=nfev; + info[8]=njev; + info[9]=nlss; + } + //sba_print_sol(n, m, p, 0, pnp, x, mnp, &idxij, rcidxs, rcsubs); + retval=(stop!=7)? itno : SBA_ERROR; + +freemem_and_return: /* NOTE: this point is also reached via a goto! */ + + /* free whatever was allocated */ + free(jac); free(V); + free(e); free(eb); + free(dp); + free(rcidxs); free(rcsubs); +#ifndef SBA_DESTROY_COVS + if(wght) free(wght); +#else + /* nothing to do */ +#endif /* SBA_DESTROY_COVS */ + + free(hx); free(diagV); free(pdp); + if(fdj_data.hxx){ // cleanup + free(fdj_data.hxx); + free(fdj_data.func_rcidxs); + } + + sba_crsm_free(&idxij); + + /* free the memory allocated by the linear solver routine */ + if(linsolver) (*linsolver)(NULL, NULL, NULL, 0, 0); + + return retval; +} diff --git a/libmoped/libs/sba-1.6/sba_levmar_wrap.c b/libmoped/libs/sba-1.6/sba_levmar_wrap.c new file mode 100644 index 0000000..afa8a04 --- /dev/null +++ b/libmoped/libs/sba-1.6/sba_levmar_wrap.c @@ -0,0 +1,896 @@ +///////////////////////////////////////////////////////////////////////////////// +//// +//// Simple drivers for sparse bundle adjustment based on the +//// Levenberg - Marquardt minimization algorithm +//// This file provides simple wrappers to the functions defined in sba_levmar.c +//// 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. +//// +/////////////////////////////////////////////////////////////////////////////////// + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> +#include <float.h> + +#include "sba.h" + + +#define FABS(x) (((x)>=0)? (x) : -(x)) + +struct wrap_motstr_data_ { + void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata); // Q + void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata); // dQ/da, dQ/db + int cnp, pnp, mnp; /* parameter numbers */ + void *adata; +}; + +struct wrap_mot_data_ { + void (*proj)(int j, int i, double *aj, double *xij, void *adata); // Q + void (*projac)(int j, int i, double *aj, double *Aij, void *adata); // dQ/da + int cnp, mnp; /* parameter numbers */ + void *adata; +}; + +struct wrap_str_data_ { + void (*proj)(int j, int i, double *bi, double *xij, void *adata); // Q + void (*projac)(int j, int i, double *bi, double *Bij, void *adata); // dQ/db + int pnp, mnp; /* parameter numbers */ + void *adata; +}; + +/* Routines to estimate the estimated measurement vector (i.e. "func") and + * its sparse jacobian (i.e. "fjac") needed by BA expert drivers. Code below + * makes use of user-supplied functions computing "Q", "dQ/da", d"Q/db", + * i.e. predicted projection and associated jacobians for a SINGLE image measurement. + * Notice also that what follows is two pairs of "func" and corresponding "fjac" routines. + * The first is to be used in full (i.e. motion + structure) BA, the second in + * motion only BA. + */ + +/* FULL BUNDLE ADJUSTMENT */ + +/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in + * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements + * are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted + * projection of the i-th point on the j-th camera. + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the hx_ij might be missing + * + */ +static void sba_motstr_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata) +{ + register int i, j; + int cnp, pnp, mnp; + double *pa, *pb, *paj, *pbi, *pxij; + int n, m, nnz; + struct wrap_motstr_data_ *wdata; + void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *proj_adata); + void *proj_adata; + + wdata=(struct wrap_motstr_data_ *)adata; + cnp=wdata->cnp; pnp=wdata->pnp; mnp=wdata->mnp; + proj=wdata->proj; + proj_adata=wdata->adata; + + n=idxij->nr; m=idxij->nc; + pa=p; pb=p+m*cnp; + + for(j=0; j<m; ++j){ + /* j-th camera parameters */ + paj=pa+j*cnp; + + nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for(i=0; i<nnz; ++i){ + pbi=pb + rcsubs[i]*pnp; + pxij=hx + idxij->val[rcidxs[i]]*mnp; // set pxij to point to hx_ij + + (*proj)(j, rcsubs[i], paj, pbi, pxij, proj_adata); // evaluate Q in pxij + } + } +} + +/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in + * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images. + * The jacobian is returned in the order (A_11, B_11, ..., A_1m, B_1m, ..., A_n1, B_n1, ..., A_nm, B_nm), + * where A_ij=dx_ij/db_j and B_ij=dx_ij/db_i (see HZ). + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the A_ij, B_ij might be missing + * + */ +static void sba_motstr_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata) +{ + register int i, j; + int cnp, pnp, mnp; + double *pa, *pb, *paj, *pbi, *pAij, *pBij; + int n, m, nnz, Asz, Bsz, ABsz, idx; + struct wrap_motstr_data_ *wdata; + void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *projac_adata); + void *projac_adata; + + + wdata=(struct wrap_motstr_data_ *)adata; + cnp=wdata->cnp; pnp=wdata->pnp; mnp=wdata->mnp; + projac=wdata->projac; + projac_adata=wdata->adata; + + n=idxij->nr; m=idxij->nc; + pa=p; pb=p+m*cnp; + Asz=mnp*cnp; Bsz=mnp*pnp; ABsz=Asz+Bsz; + + for(j=0; j<m; ++j){ + /* j-th camera parameters */ + paj=pa+j*cnp; + + nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for(i=0; i<nnz; ++i){ + pbi=pb + rcsubs[i]*pnp; + idx=idxij->val[rcidxs[i]]; + pAij=jac + idx*ABsz; // set pAij to point to A_ij + pBij=pAij + Asz; // set pBij to point to B_ij + + (*projac)(j, rcsubs[i], paj, pbi, pAij, pBij, projac_adata); // evaluate dQ/da, dQ/db in pAij, pBij + } + } +} + +/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in + * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images. + * The jacobian is approximated with the aid of finite differences and is returned in the order + * (A_11, B_11, ..., A_1m, B_1m, ..., A_n1, B_n1, ..., A_nm, B_nm), + * where A_ij=dx_ij/da_j and B_ij=dx_ij/db_i (see HZ). + * Notice that depending on idxij, some of the A_ij, B_ij might be missing + * + * Problem-specific information is assumed to be stored in a structure pointed to by "dat". + * + * NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern, + * the jacobian should be computed analytically + */ +static void sba_motstr_Qs_fdjac( + double *p, /* I: current parameter estimate, (m*cnp+n*pnp)x1 */ + struct sba_crsm *idxij, /* I: sparse matrix containing the location of x_ij in hx */ + int *rcidxs, /* work array for the indexes of nonzero elements of a single sparse matrix row/column */ + int *rcsubs, /* work array for the subscripts of nonzero elements in a single sparse matrix row/column */ + double *jac, /* O: array for storing the approximated jacobian */ + void *dat) /* I: points to a "wrap_motstr_data_" structure */ +{ + register int i, j, ii, jj; + double *pa, *pb, *paj, *pbi; + register double *pAB; + int n, m, nnz, Asz, Bsz, ABsz; + + double tmp; + register double d, d1; + + struct wrap_motstr_data_ *fdjd; + void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata); + double *hxij, *hxxij; + int cnp, pnp, mnp; + void *adata; + + /* retrieve problem-specific information passed in *dat */ + fdjd=(struct wrap_motstr_data_ *)dat; + proj=fdjd->proj; + cnp=fdjd->cnp; pnp=fdjd->pnp; mnp=fdjd->mnp; + adata=fdjd->adata; + + n=idxij->nr; m=idxij->nc; + pa=p; pb=p+m*cnp; + Asz=mnp*cnp; Bsz=mnp*pnp; ABsz=Asz+Bsz; + + /* allocate memory for hxij, hxxij */ + if((hxij=malloc(2*mnp*sizeof(double)))==NULL){ + fprintf(stderr, "memory allocation request failed in sba_motstr_Qs_fdjac()!\n"); + exit(1); + } + hxxij=hxij+mnp; + + /* compute A_ij */ + for(j=0; j<m; ++j){ + paj=pa+j*cnp; // j-th camera parameters + + nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero A_ij, i=0...n-1 */ + for(jj=0; jj<cnp; ++jj){ + /* determine d=max(SBA_DELTA_SCALE*|paj[jj]|, SBA_MIN_DELTA), see HZ */ + d=(double)(SBA_DELTA_SCALE)*paj[jj]; // force evaluation + d=FABS(d); + if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA; + d1=1.0/d; /* invert so that divisions can be carried out faster as multiplications */ + + for(i=0; i<nnz; ++i){ + pbi=pb + rcsubs[i]*pnp; // i-th point parameters + (*proj)(j, rcsubs[i], paj, pbi, hxij, adata); // evaluate supplied function on current solution + + tmp=paj[jj]; + paj[jj]+=d; + (*proj)(j, rcsubs[i], paj, pbi, hxxij, adata); + paj[jj]=tmp; /* restore */ + + pAB=jac + idxij->val[rcidxs[i]]*ABsz; // set pAB to point to A_ij + for(ii=0; ii<mnp; ++ii) + pAB[ii*cnp+jj]=(hxxij[ii]-hxij[ii])*d1; + } + } + } + + /* compute B_ij */ + for(i=0; i<n; ++i){ + pbi=pb+i*pnp; // i-th point parameters + + nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero B_ij, j=0...m-1 */ + for(jj=0; jj<pnp; ++jj){ + /* determine d=max(SBA_DELTA_SCALE*|pbi[jj]|, SBA_MIN_DELTA), see HZ */ + d=(double)(SBA_DELTA_SCALE)*pbi[jj]; // force evaluation + d=FABS(d); + if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA; + d1=1.0/d; /* invert so that divisions can be carried out faster as multiplications */ + + for(j=0; j<nnz; ++j){ + paj=pa + rcsubs[j]*cnp; // j-th camera parameters + (*proj)(rcsubs[j], i, paj, pbi, hxij, adata); // evaluate supplied function on current solution + + tmp=pbi[jj]; + pbi[jj]+=d; + (*proj)(rcsubs[j], i, paj, pbi, hxxij, adata); + pbi[jj]=tmp; /* restore */ + + pAB=jac + idxij->val[rcidxs[j]]*ABsz + Asz; // set pAB to point to B_ij + for(ii=0; ii<mnp; ++ii) + pAB[ii*pnp+jj]=(hxxij[ii]-hxij[ii])*d1; + } + } + } + + free(hxij); +} + +/* BUNDLE ADJUSTMENT FOR CAMERA PARAMETERS ONLY */ + +/* Given a parameter vector p made up of the parameters of m cameras, compute in + * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. + * The measurements are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, + * where hx_ij is the predicted projection of the i-th point on the j-th camera. + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the hx_ij might be missing + * + */ +static void sba_mot_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata) +{ + register int i, j; + int cnp, mnp; + double *paj, *pxij; + //int n; + int m, nnz; + struct wrap_mot_data_ *wdata; + void (*proj)(int j, int i, double *aj, double *xij, void *proj_adata); + void *proj_adata; + + wdata=(struct wrap_mot_data_ *)adata; + cnp=wdata->cnp; mnp=wdata->mnp; + proj=wdata->proj; + proj_adata=wdata->adata; + + //n=idxij->nr; + m=idxij->nc; + + for(j=0; j<m; ++j){ + /* j-th camera parameters */ + paj=p+j*cnp; + + nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for(i=0; i<nnz; ++i){ + pxij=hx + idxij->val[rcidxs[i]]*mnp; // set pxij to point to hx_ij + + (*proj)(j, rcsubs[i], paj, pxij, proj_adata); // evaluate Q in pxij + } + } +} + +/* Given a parameter vector p made up of the parameters of m cameras, compute in jac + * the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images. + * The jacobian is returned in the order (A_11, ..., A_1m, ..., A_n1, ..., A_nm), + * where A_ij=dx_ij/db_j (see HZ). + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the A_ij might be missing + * + */ +static void sba_mot_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata) +{ + register int i, j; + int cnp, mnp; + double *paj, *pAij; + //int n; + int m, nnz, Asz, idx; + struct wrap_mot_data_ *wdata; + void (*projac)(int j, int i, double *aj, double *Aij, void *projac_adata); + void *projac_adata; + + wdata=(struct wrap_mot_data_ *)adata; + cnp=wdata->cnp; mnp=wdata->mnp; + projac=wdata->projac; + projac_adata=wdata->adata; + + //n=idxij->nr; + m=idxij->nc; + Asz=mnp*cnp; + + for(j=0; j<m; ++j){ + /* j-th camera parameters */ + paj=p+j*cnp; + + nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for(i=0; i<nnz; ++i){ + idx=idxij->val[rcidxs[i]]; + pAij=jac + idx*Asz; // set pAij to point to A_ij + + (*projac)(j, rcsubs[i], paj, pAij, projac_adata); // evaluate dQ/da in pAij + } + } +} + +/* Given a parameter vector p made up of the parameters of m cameras, compute in jac the jacobian + * of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images. + * The jacobian is approximated with the aid of finite differences and is returned in the order + * (A_11, ..., A_1m, ..., A_n1, ..., A_nm), where A_ij=dx_ij/da_j (see HZ). + * Notice that depending on idxij, some of the A_ij might be missing + * + * Problem-specific information is assumed to be stored in a structure pointed to by "dat". + * + * NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern, + * the jacobian should be computed analytically + */ +static void sba_mot_Qs_fdjac( + double *p, /* I: current parameter estimate, (m*cnp)x1 */ + struct sba_crsm *idxij, /* I: sparse matrix containing the location of x_ij in hx */ + int *rcidxs, /* work array for the indexes of nonzero elements of a single sparse matrix row/column */ + int *rcsubs, /* work array for the subscripts of nonzero elements in a single sparse matrix row/column */ + double *jac, /* O: array for storing the approximated jacobian */ + void *dat) /* I: points to a "wrap_mot_data_" structure */ +{ + register int i, j, ii, jj; + double *paj; + register double *pA; + //int n; + int m, nnz, Asz; + + double tmp; + register double d, d1; + + struct wrap_mot_data_ *fdjd; + void (*proj)(int j, int i, double *aj, double *xij, void *adata); + double *hxij, *hxxij; + int cnp, mnp; + void *adata; + + /* retrieve problem-specific information passed in *dat */ + fdjd=(struct wrap_mot_data_ *)dat; + proj=fdjd->proj; + cnp=fdjd->cnp; mnp=fdjd->mnp; + adata=fdjd->adata; + + //n=idxij->nr; + m=idxij->nc; + Asz=mnp*cnp; + + /* allocate memory for hxij, hxxij */ + if((hxij=malloc(2*mnp*sizeof(double)))==NULL){ + fprintf(stderr, "memory allocation request failed in sba_mot_Qs_fdjac()!\n"); + exit(1); + } + hxxij=hxij+mnp; + + /* compute A_ij */ + for(j=0; j<m; ++j){ + paj=p+j*cnp; // j-th camera parameters + + nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero A_ij, i=0...n-1 */ + for(jj=0; jj<cnp; ++jj){ + /* determine d=max(SBA_DELTA_SCALE*|paj[jj]|, SBA_MIN_DELTA), see HZ */ + d=(double)(SBA_DELTA_SCALE)*paj[jj]; // force evaluation + d=FABS(d); + if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA; + d1=1.0/d; /* invert so that divisions can be carried out faster as multiplications */ + + for(i=0; i<nnz; ++i){ + (*proj)(j, rcsubs[i], paj, hxij, adata); // evaluate supplied function on current solution + + tmp=paj[jj]; + paj[jj]+=d; + (*proj)(j, rcsubs[i], paj, hxxij, adata); + paj[jj]=tmp; /* restore */ + + pA=jac + idxij->val[rcidxs[i]]*Asz; // set pA to point to A_ij + for(ii=0; ii<mnp; ++ii) + pA[ii*cnp+jj]=(hxxij[ii]-hxij[ii])*d1; + } + } + } + + free(hxij); +} + +/* BUNDLE ADJUSTMENT FOR STRUCTURE PARAMETERS ONLY */ + +/* Given a parameter vector p made up of the 3D coordinates of n points, compute in + * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements + * are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted + * projection of the i-th point on the j-th camera. + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the hx_ij might be missing + * + */ +static void sba_str_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata) +{ + register int i, j; + int pnp, mnp; + double *pbi, *pxij; + //int n; + int m, nnz; + struct wrap_str_data_ *wdata; + void (*proj)(int j, int i, double *bi, double *xij, void *proj_adata); + void *proj_adata; + + wdata=(struct wrap_str_data_ *)adata; + pnp=wdata->pnp; mnp=wdata->mnp; + proj=wdata->proj; + proj_adata=wdata->adata; + + //n=idxij->nr; + m=idxij->nc; + + for(j=0; j<m; ++j){ + nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for(i=0; i<nnz; ++i){ + pbi=p + rcsubs[i]*pnp; + pxij=hx + idxij->val[rcidxs[i]]*mnp; // set pxij to point to hx_ij + + (*proj)(j, rcsubs[i], pbi, pxij, proj_adata); // evaluate Q in pxij + } + } +} + +/* Given a parameter vector p made up of the 3D coordinates of n points, compute in + * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images. + * The jacobian is returned in the order (B_11, ..., B_1m, ..., B_n1, ..., B_nm), where B_ij=dx_ij/db_i (see HZ). + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the B_ij might be missing + * + */ +static void sba_str_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata) +{ + register int i, j; + int pnp, mnp; + double *pbi, *pBij; + //int n; + int m, nnz, Bsz, idx; + struct wrap_str_data_ *wdata; + void (*projac)(int j, int i, double *bi, double *Bij, void *projac_adata); + void *projac_adata; + + + wdata=(struct wrap_str_data_ *)adata; + pnp=wdata->pnp; mnp=wdata->mnp; + projac=wdata->projac; + projac_adata=wdata->adata; + + //n=idxij->nr; + m=idxij->nc; + Bsz=mnp*pnp; + + for(j=0; j<m; ++j){ + + nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for(i=0; i<nnz; ++i){ + pbi=p + rcsubs[i]*pnp; + idx=idxij->val[rcidxs[i]]; + pBij=jac + idx*Bsz; // set pBij to point to B_ij + + (*projac)(j, rcsubs[i], pbi, pBij, projac_adata); // evaluate dQ/db in pBij + } + } +} + +/* Given a parameter vector p made up of the 3D coordinates of n points, compute in + * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images. + * The jacobian is approximated with the aid of finite differences and is returned in the order + * (B_11, ..., B_1m, ..., B_n1, ..., B_nm), where B_ij=dx_ij/db_i (see HZ). + * Notice that depending on idxij, some of the B_ij might be missing + * + * Problem-specific information is assumed to be stored in a structure pointed to by "dat". + * + * NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern, + * the jacobian should be computed analytically + */ +static void sba_str_Qs_fdjac( + double *p, /* I: current parameter estimate, (n*pnp)x1 */ + struct sba_crsm *idxij, /* I: sparse matrix containing the location of x_ij in hx */ + int *rcidxs, /* work array for the indexes of nonzero elements of a single sparse matrix row/column */ + int *rcsubs, /* work array for the subscripts of nonzero elements in a single sparse matrix row/column */ + double *jac, /* O: array for storing the approximated jacobian */ + void *dat) /* I: points to a "wrap_str_data_" structure */ +{ + register int i, j, ii, jj; + double *pbi; + register double *pB; + //int m; + int n, nnz, Bsz; + + double tmp; + register double d, d1; + + struct wrap_str_data_ *fdjd; + void (*proj)(int j, int i, double *bi, double *xij, void *adata); + double *hxij, *hxxij; + int pnp, mnp; + void *adata; + + /* retrieve problem-specific information passed in *dat */ + fdjd=(struct wrap_str_data_ *)dat; + proj=fdjd->proj; + pnp=fdjd->pnp; mnp=fdjd->mnp; + adata=fdjd->adata; + + n=idxij->nr; + //m=idxij->nc; + Bsz=mnp*pnp; + + /* allocate memory for hxij, hxxij */ + if((hxij=malloc(2*mnp*sizeof(double)))==NULL){ + fprintf(stderr, "memory allocation request failed in sba_str_Qs_fdjac()!\n"); + exit(1); + } + hxxij=hxij+mnp; + + /* compute B_ij */ + for(i=0; i<n; ++i){ + pbi=p+i*pnp; // i-th point parameters + + nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero B_ij, j=0...m-1 */ + for(jj=0; jj<pnp; ++jj){ + /* determine d=max(SBA_DELTA_SCALE*|pbi[jj]|, SBA_MIN_DELTA), see HZ */ + d=(double)(SBA_DELTA_SCALE)*pbi[jj]; // force evaluation + d=FABS(d); + if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA; + d1=1.0/d; /* invert so that divisions can be carried out faster as multiplications */ + + for(j=0; j<nnz; ++j){ + (*proj)(rcsubs[j], i, pbi, hxij, adata); // evaluate supplied function on current solution + + tmp=pbi[jj]; + pbi[jj]+=d; + (*proj)(rcsubs[j], i, pbi, hxxij, adata); + pbi[jj]=tmp; /* restore */ + + pB=jac + idxij->val[rcidxs[j]]*Bsz; // set pB to point to B_ij + for(ii=0; ii<mnp; ++ii) + pB[ii*pnp+jj]=(hxxij[ii]-hxij[ii])*d1; + } + } + } + + free(hxij); +} + + +/* + * Simple driver to sba_motstr_levmar_x for bundle adjustment on camera and structure parameters. + * + * Returns the number of iterations (>=0) if successfull, SBA_ERROR if failed + */ + +int sba_motstr_levmar( + const int n, /* number of points */ + const int ncon,/* number of points (starting from the 1st) whose parameters should not be modified. + * All B_ij (see below) with i<ncon are assumed to be zero + */ + const int m, /* number of images */ + const int mcon,/* number of images (starting from the 1st) whose parameters should not be modified. + * All A_ij (see below) with j<mcon are assumed to be zero + */ + char *vmask, /* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */ + double *p, /* initial parameter vector p0: (a1, ..., am, b1, ..., bn). + * aj are the image j parameters, bi are the i-th point parameters, + * size m*cnp + n*pnp + */ + const int cnp,/* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */ + const int pnp,/* number of parameters for ONE point; e.g. 3 for Euclidean points */ + double *x, /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where + * x_ij is the projection of the i-th point on the j-th image. + * NOTE: some of the x_ij might be missing, if point i is not visible in image j; + * see vmask[i, j], max. size n*m*mnp + */ + double *covx, /* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm), + * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no + * covariance estimates are available (identity matrices are implicitly used in this case). + * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing; + * see vmask[i, j], max. size n*m*mnp*mnp + */ + const int mnp,/* number of parameters for EACH measurement; usually 2 */ + void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata), + /* functional relation computing a SINGLE image measurement. Assuming that + * the parameters of point i are bi and the parameters of camera j aj, + * computes a prediction of \hat{x}_{ij}. aj is cnp x 1, bi is pnp x 1 and + * xij is mnp x 1. This function is called only if point i is visible in + * image j (i.e. vmask[i, j]==1) + */ + void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata), + /* functional relation to evaluate d x_ij / d a_j and + * d x_ij / d b_i in Aij and Bij resp. + * This function is called only if point i is visible in * image j + * (i.e. vmask[i, j]==1). Also, A_ij and B_ij are mnp x cnp and mnp x pnp + * matrices resp. and they should be stored in row-major order. + * + * If NULL, the jacobians are approximated by repetitive proj calls + * and finite differences. + */ + void *adata, /* pointer to possibly additional data, passed uninterpreted to proj, projac */ + + const int itmax, /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */ + const int verbose, /* I: verbosity */ + const double opts[SBA_OPTSSZ], + /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial \mu, + * stoping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2 + */ + double info[SBA_INFOSZ] + /* O: information regarding the minimization. Set to NULL if don't care + * info[0]=||e||_2 at initial p. + * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. + * info[5]= # iterations, + * info[6]=reason for terminating: 1 - stopped by small gradient J^T e + * 2 - stopped by small dp + * 3 - stopped by itmax + * 4 - stopped by small relative reduction in ||e||_2 + * 5 - too many attempts to increase damping. Restart with increased mu + * 6 - stopped by small ||e||_2 + * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error + * info[7]= # function evaluations + * info[8]= # jacobian evaluations + * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error + */ +) +{ +int retval; +struct wrap_motstr_data_ wdata; +static void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata); + + wdata.proj=proj; + wdata.projac=projac; + wdata.cnp=cnp; + wdata.pnp=pnp; + wdata.mnp=mnp; + wdata.adata=adata; + + fjac=(projac)? sba_motstr_Qs_jac : sba_motstr_Qs_fdjac; + retval=sba_motstr_levmar_x(n, ncon, m, mcon, vmask, p, cnp, pnp, x, covx, mnp, sba_motstr_Qs, fjac, &wdata, itmax, verbose, opts, info); + + if(info){ + register int i; + int nvis; + + /* count visible image points */ + for(i=nvis=0; i<n*m; ++i) + nvis+=(vmask[i]!=0); + + /* each "func" & "fjac" evaluation requires nvis "proj" & "projac" evaluations */ + info[7]*=nvis; + info[8]*=nvis; + } + + return retval; +} + + +/* + * Simple driver to sba_mot_levmar_x for bundle adjustment on camera parameters. + * + * Returns the number of iterations (>=0) if successfull, SBA_ERROR if failed + */ + +int sba_mot_levmar( + const int n, /* number of points */ + const int m, /* number of images */ + const int mcon,/* number of images (starting from the 1st) whose parameters should not be modified. + * All A_ij (see below) with j<mcon are assumed to be zero + */ + char *vmask, /* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */ + double *p, /* initial parameter vector p0: (a1, ..., am). + * aj are the image j parameters, size m*cnp */ + const int cnp,/* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */ + double *x, /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where + * x_ij is the projection of the i-th point on the j-th image. + * NOTE: some of the x_ij might be missing, if point i is not visible in image j; + * see vmask[i, j], max. size n*m*mnp + */ + double *covx, /* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm), + * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no + * covariance estimates are available (identity matrices are implicitly used in this case). + * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing; + * see vmask[i, j], max. size n*m*mnp*mnp + */ + const int mnp,/* number of parameters for EACH measurement; usually 2 */ + void (*proj)(int j, int i, double *aj, double *xij, void *adata), + /* functional relation computing a SINGLE image measurement. Assuming that + * the parameters of camera j are aj, computes a prediction of \hat{x}_{ij} + * for point i. aj is cnp x 1 and xij is mnp x 1. + * This function is called only if point i is visible in image j (i.e. vmask[i, j]==1) + */ + void (*projac)(int j, int i, double *aj, double *Aij, void *adata), + /* functional relation to evaluate d x_ij / d a_j in Aij + * This function is called only if point i is visible in image j + * (i.e. vmask[i, j]==1). Also, A_ij are a mnp x cnp matrices + * and should be stored in row-major order. + * + * If NULL, the jacobian is approximated by repetitive proj calls + * and finite differences. + */ + void *adata, /* pointer to possibly additional data, passed uninterpreted to proj, projac */ + + const int itmax, /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */ + const int verbose, /* I: verbosity */ + const double opts[SBA_OPTSSZ], + /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon]. Respectively the scale factor for initial \mu, + * stoping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2 + */ + double info[SBA_INFOSZ] + /* O: information regarding the minimization. Set to NULL if don't care + * info[0]=||e||_2 at initial p. + * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. + * info[5]= # iterations, + * info[6]=reason for terminating: 1 - stopped by small gradient J^T e + * 2 - stopped by small dp + * 3 - stopped by itmax + * 4 - stopped by small relative reduction in ||e||_2 + * 5 - too many attempts to increase damping. Restart with increased mu + * 6 - stopped by small ||e||_2 + * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error + * info[7]= # function evaluations + * info[8]= # jacobian evaluations + * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error + */ +) +{ +int retval; +struct wrap_mot_data_ wdata; +void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata); + + wdata.proj=proj; + wdata.projac=projac; + wdata.cnp=cnp; + wdata.mnp=mnp; + wdata.adata=adata; + + fjac=(projac)? sba_mot_Qs_jac : sba_mot_Qs_fdjac; + retval=sba_mot_levmar_x(n, m, mcon, vmask, p, cnp, x, covx, mnp, sba_mot_Qs, fjac, &wdata, itmax, verbose, opts, info); + + if(info){ + register int i; + int nvis; + + /* count visible image points */ + for(i=nvis=0; i<n*m; ++i) + nvis+=(vmask[i]!=0); + + /* each "func" & "fjac" evaluation requires nvis "proj" & "projac" evaluations */ + info[7]*=nvis; + info[8]*=nvis; + } + + return retval; +} + +/* + * Simple driver to sba_str_levmar_x for bundle adjustment on structure parameters. + * + * Returns the number of iterations (>=0) if successfull, SBA_ERROR if failed + */ + +int sba_str_levmar( + const int n, /* number of points */ + const int ncon,/* number of points (starting from the 1st) whose parameters should not be modified. + * All B_ij (see below) with i<ncon are assumed to be zero + */ + const int m, /* number of images */ + char *vmask, /* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */ + double *p, /* initial parameter vector p0: (b1, ..., bn). + * bi are the i-th point parameters, size n*pnp + */ + const int pnp,/* number of parameters for ONE point; e.g. 3 for Euclidean points */ + double *x, /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where + * x_ij is the projection of the i-th point on the j-th image. + * NOTE: some of the x_ij might be missing, if point i is not visible in image j; + * see vmask[i, j], max. size n*m*mnp + */ + double *covx, /* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm), + * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no + * covariance estimates are available (identity matrices are implicitly used in this case). + * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing; + * see vmask[i, j], max. size n*m*mnp*mnp + */ + const int mnp,/* number of parameters for EACH measurement; usually 2 */ + void (*proj)(int j, int i, double *bi, double *xij, void *adata), + /* functional relation computing a SINGLE image measurement. Assuming that + * the parameters of point i are bi, computes a prediction of \hat{x}_{ij}. + * bi is pnp x 1 and xij is mnp x 1. This function is called only if point + * i is visible in image j (i.e. vmask[i, j]==1) + */ + void (*projac)(int j, int i, double *bi, double *Bij, void *adata), + /* functional relation to evaluate d x_ij / d b_i in Bij. + * This function is called only if point i is visible in image j + * (i.e. vmask[i, j]==1). Also, B_ij are mnp x pnp matrices + * and they should be stored in row-major order. + * + * If NULL, the jacobians are approximated by repetitive proj calls + * and finite differences. + */ + void *adata, /* pointer to possibly additional data, passed uninterpreted to proj, projac */ + + const int itmax, /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */ + const int verbose, /* I: verbosity */ + const double opts[SBA_OPTSSZ], + /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial \mu, + * stoping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2 + */ + double info[SBA_INFOSZ] + /* O: information regarding the minimization. Set to NULL if don't care + * info[0]=||e||_2 at initial p. + * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. + * info[5]= # iterations, + * info[6]=reason for terminating: 1 - stopped by small gradient J^T e + * 2 - stopped by small dp + * 3 - stopped by itmax + * 4 - stopped by small relative reduction in ||e||_2 + * 5 - too many attempts to increase damping. Restart with increased mu + * 6 - stopped by small ||e||_2 + * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error + * info[7]= # function evaluations + * info[8]= # jacobian evaluations + * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error + */ +) +{ +int retval; +struct wrap_str_data_ wdata; +static void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata); + + wdata.proj=proj; + wdata.projac=projac; + wdata.pnp=pnp; + wdata.mnp=mnp; + wdata.adata=adata; + + fjac=(projac)? sba_str_Qs_jac : sba_str_Qs_fdjac; + retval=sba_str_levmar_x(n, ncon, m, vmask, p, pnp, x, covx, mnp, sba_str_Qs, fjac, &wdata, itmax, verbose, opts, info); + + if(info){ + register int i; + int nvis; + + /* count visible image points */ + for(i=nvis=0; i<n*m; ++i) + nvis+=(vmask[i]!=0); + + /* each "func" & "fjac" evaluation requires nvis "proj" & "projac" evaluations */ + info[7]*=nvis; + info[8]*=nvis; + } + + return retval; +} diff --git a/libmoped/libs/sba-1.6/utils/README.txt b/libmoped/libs/sba-1.6/utils/README.txt new file mode 100644 index 0000000..0f56a3c --- /dev/null +++ b/libmoped/libs/sba-1.6/utils/README.txt @@ -0,0 +1,37 @@ +==================== GENERAL ==================== +This directory contains utilities related to sba. + +==================== FILES ==================== +hess2eps.c: C source of a function for visualizing the sparseness pattern + of JtJ in EPS format. +SBAio.pm: Perl module with functions for loading a 3D reconstruction. + The format of datafiles is that explained in ../demo/README.txt +reprerr.pl: Perl script for computing the reprojection error corresponding + to a 3D reconstruction +quats.pl: Perl utility functions for quaternions + +==================== SAMPLE RUNS ==================== +Consult http://www.ics.forth.gr/~lourakis/sba/bt_pattern.pdf +for sample output generated with sba_hessian2eps() for Oxford's +corridor sequence. + + +The command + ./reprerr.pl -e ../demo/7cams.txt ../demo/7pts.txt ../demo/calib.txt +produces the following output: + +Read 7 cameras +Read 465 3D points & trajectories +Mean error for camera 0 [207 projections] is 0.420131 +Mean error for camera 1 [244 projections] is 0.46823 +Mean error for camera 2 [302 projections] is 0.727632 +Mean error for camera 3 [352 projections] is 1.04241 +Mean error for camera 4 [351 projections] is 1.14576 +Mean error for camera 5 [274 projections] is 1.68392 +Mean error for camera 6 [186 projections] is 12.4378 + +Mean error for the whole sequence [1916 projections] is 2.06935 + + + +Send your comments/questions to lourakis@ics.forth.gr diff --git a/libmoped/libs/sba-1.6/utils/SBAio.pm b/libmoped/libs/sba-1.6/utils/SBAio.pm new file mode 100644 index 0000000..5e3be9a --- /dev/null +++ b/libmoped/libs/sba-1.6/utils/SBAio.pm @@ -0,0 +1,239 @@ +################################################################################# +## +## Perl script for reading reconstructions in SBA's file format +## Copyright (C) 2005-9 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. +## +################################################################################# + +package SBAio; + +use strict; + +# NOTE: all 2D arrays below are stored in row-major order as vectors + +# read calibration file containing 3x3 array +sub readCalib{ + my ($calfile)=@_; + my ($i, $line, @columns, @camCal); + + if(length($calfile)>0){ + if(not open(CAL, $calfile)){ + print STDERR "cannot open file $calfile: $!\n"; + exit(1); + } + @camCal=(); + for($i=0; $i<3; ){ # $i gets incremented at the bottom of the loop + $line=<CAL>; + if($line=~/\r\n$/){ # CR+LF + chop($line); chop($line); + } + else{ + chomp($line); + } + + next if($line=~/^#.+/); # skip comments + + @columns=split(" ", $line); + die "line \"$line\" in $calfile does not contain exactly 3 numbers [$#columns+1]!\n" if($#columns+1!=3); + $camCal[$i*3]=$columns[0]; $camCal[$i*3+1]=$columns[1]; $camCal[$i*3+2]=$columns[2]; + $i++; + } + close(CAL); + } + + return [@camCal]; +} + +# read cameras file +sub readCameras{ + my ($camsfile, $cnp)=@_; + my($i, $line, $ncams, @columns, @pose, @camParams); + + if(not open(CAMS, $camsfile)){ + print STDERR "cannot open file $camsfile: $!\n"; + exit(1); + } + @camParams=(); # array of arrays storing each camera's parameters + $ncams=0; + while($line=<CAMS>){ + if($line=~/\r\n$/){ # CR+LF + chop($line); chop($line); + } + else{ + chomp($line); + } + + next if($line=~/^#.+/); # skip comments + + @columns=split(" ", $line); + #next if($#columns==-1); # skip empty lines + + die "line \"$line\" in $camsfile does not contain exactly $cnp numbers [$#columns+1]!\n" if($cnp!=$#columns+1); + @pose=(); + for($i=0; $i<$cnp; $i++){ + $pose[$i]=$columns[$i]; + } + $camParams[$ncams++]=[@pose]; + } + close(CAMS); + + return [@camParams]; +} + +# read points file +sub readPoints{ + my($ptsfile, $pnp)=@_; + my($i, $j, $line, $npts, $totprojs, @columns, $nframes, $fr, %traj, @recpt, @theframes, @ptParams, @imgTrajs, @trajFrames); + + if(not open(PTS, $ptsfile)){ + print STDERR "cannot open file $ptsfile: $!\n"; + exit(1); + } + + @ptParams=(); # array of arrays storing the reconstructed 3D points; each element is of size $pnp + @imgTrajs=(); # array of hashes storing the 2D trajectory correponding to reconstructed 3D points. + # The hash key is the frame number + @trajFrames=(); # array of arrays storing the frame numbers corresponding to each trajectory. + # The first number is the total number of frames, then follow the individual frame + $npts=0; + $totprojs=0; + while($line=<PTS>){ + if($line=~/\r\n$/){ # CR+LF + chop($line); chop($line); + } + else{ + chomp($line); + } + + next if($line=~/^#.+/); # skip comments + @columns=split(" ", $line); + + die "line \"$line\" in $ptsfile contains less than $pnp numbers [$#columns+1]!\n" if($pnp>$#columns+1); + @recpt=(); + for($i=0; $i<$pnp; $i++){ + $recpt[$i]=$columns[$i]; + } + + $nframes=$columns[$pnp]; + $i=$pnp+1+$nframes*3; # 3 numbers per image projection: (i.e. imgid, x, y) + if($i!=$#columns+1){ + die "line \"$line\" in $ptsfile does not contain exactly the $i numbers required for a 3D point with $nframes 2D projections [$#columns+1]!\n"; + } + + printf STDERR "Warning: point %d (%s) has no image projections!\n", $npts, $line, if($nframes<=0); + + %traj=(); + @theframes=($nframes); + $totprojs=$totprojs+$nframes; + for($i=0, $j=$pnp+1; $i<$nframes; $i++, $j+=3){ + $fr=$columns[$j]; +# if($fr>0){ +# $fr =~ s/^0+//; # drop any leading zeros +# } +# else{ +# $fr=0; # avoid 00, 000, etc... +# } + $fr =~ s/^0+//; # remove any leading zeros + $fr =~ s/^$/0/; # put one back if they're all zero + + $traj{$fr}=[$columns[$j+1], $columns[$j+2]]; + push @theframes, $fr; + +# printf "%d: %d %.4g %.4g\n", $j, $fr, $columns[$j+1], $columns[$j+2]; + } + $ptParams[$npts]=[@recpt]; + $imgTrajs[$npts]={%traj}; + $trajFrames[$npts++]=[@theframes]; + } + close(PTS); + + return ([@ptParams], [@imgTrajs], [@trajFrames], $totprojs); +} + + +# subroutine showing how the read reconstruction can be printed +sub printRecons{ + my($camParams, $cnp, $ptParams, $imgTrajs, $trajFrames, $pnp)=@_; + my($i, $j, $fr); + + for($i=0; $i<scalar(@$camParams); $i++){ + for($j=0; $j<$cnp; $j++){ + printf "%.8e ", $camParams->[$i][$j]; + } + print "\n"; + } + + print "\n\n"; + + for($i=0; $i<scalar(@$ptParams); $i++){ + for($j=0; $j<$pnp; $j++){ + printf "%.8e ", $ptParams->[$i][$j]; + } + + printf "%d ", $trajFrames->[$i][0]; + for($j=0; $j<$trajFrames->[$i][0]; $j++){ + $fr=$trajFrames->[$i][$j+1]; + + # sanity check + #die "internal inconsistency for point %d in printRecons()!\n", $i if(!defined($imgTrajs->[$i]{$fr})); + + printf "%d %.4e %.4e ", $fr, $imgTrajs->[$i]{$fr}[0], $imgTrajs->[$i]{$fr}[1]; + } + print "\n"; + } +} + +# find the matches between a pair of images. returns a 2D array whose each line +# is reftoproj1, reftoproj2, ptID +# +# example of use: +# +#$pairs=SBAio::getMatchPairs($imgtrajs, 0, 2); +#for($i=0; $i<scalar(@$pairs); $i++){ +# printf "%.4e %.4e %.4e %.4e [%d]\n", $pairs->[$i][0]->[0], $pairs->[$i][0]->[1], $pairs->[$i][1]->[0], $pairs->[$i][1]->[1], $pairs->[$i][2]; +#} +# +sub getMatchPairs{ + my ($imgTrajs, $fr1, $fr2)=@_; + my ($i, @matches); + + @matches=(); + for($i=0; $i<scalar(@$imgTrajs); $i++){ + if(defined($imgTrajs->[$i]{$fr1}) && defined($imgTrajs->[$i]{$fr2})){ + push @matches, [$imgTrajs->[$i]{$fr1}, $imgTrajs->[$i]{$fr2}, $i]; + } + } + + return [@matches]; +} + +# similar to the above for a triplet of images. returns a 2D array whose each line +# is reftoproj1, reftoproj2, reftoproj3, ptID +sub getMatchTriplets{ + my ($imgTrajs, $fr1, $fr2, $fr3)=@_; + my ($i, @matches); + + @matches=(); + for($i=0; $i<scalar(@$imgTrajs); $i++){ + if(defined($imgTrajs->[$i]{$fr1}) && defined($imgTrajs->[$i]{$fr2}) && defined($imgTrajs->[$i]{$fr3})){ + push @matches, [$imgTrajs->[$i]{$fr1}, $imgTrajs->[$i]{$fr2}, $imgTrajs->[$i]{$fr3}, $i]; + } + } + + return [@matches]; +} + + +1; # return true diff --git a/libmoped/libs/sba-1.6/utils/hess2eps.c b/libmoped/libs/sba-1.6/utils/hess2eps.c new file mode 100644 index 0000000..ae868e2 --- /dev/null +++ b/libmoped/libs/sba-1.6/utils/hess2eps.c @@ -0,0 +1,144 @@ +///////////////////////////////////////////////////////////////////////////////// +//// +//// Visualization of the nonzero pattern of JtJ in EPS format. +//// This is part of the sba package, +//// Copyright (C) 2004 Manolis Lourakis (lourakis at ics.forth.gr) +//// Institute of Computer Science, Foundation for Research & Technology - Hellas +//// Heraklion, Crete, Greece. +//// +//// This program is free software; you can redistribute it and/or modify +//// it under the terms of the GNU General Public License as published by +//// the Free Software Foundation; either version 2 of the License, or +//// (at your option) any later version. +//// +//// This program is distributed in the hope that it will be useful, +//// but WITHOUT ANY WARRANTY; without even the implied warranty of +//// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +//// GNU General Public License for more details. +//// +/////////////////////////////////////////////////////////////////////////////////// + +#include <stdio.h> +#include <stdlib.h> +#include <math.h> +#include <time.h> + +#include "sba.h" + +#if 0 +#define HORZ_OFFSET 3 // horizontal offset from left edge of paper in PostScript points (~ 1 mm) +#define VERT_OFFSET 6 // vertical offset from lower edge of paper in PostScript points (~ 2 mm) +#else +#define HORZ_OFFSET 0 +#define VERT_OFFSET 0 +#endif + +#define A4_WIDTH 595 // A4 paper width in PostScript points (210 mm, 1 pt = 25.4/72 mm) +//#define A4_HEIGHT 842 // A4 paper height in PostScript points (297 mm) + +/* 1.1x1.1 looks better than 1.0x1.0 */ +#define DOT_WIDTH 1.1 +#define DOT_HEIGHT 1.1 + +/* saves the nonzero pattern of JtJ in EPS format. If mcon!=0, then all U_j and W_ij with j<mcon + * are assumed to be zero and are therefore clipped off. + */ +void sba_hessian2eps(int n, int m, int mcon, int cnp, int pnp, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, + double *U, double *V, double *W, char *fname) +{ +register int i, j, ii, jj; +int i0, j0; +int mmcon, nvars, Usz, Vsz, Wsz, nnz, bbox=1, denseblocks=0; +register double *ptr1, *ptr2; +FILE *fp; +time_t tim; + + mmcon=m-mcon; + nvars=mmcon*cnp + n*pnp; + Usz=cnp * cnp; Vsz=pnp * pnp; Wsz=cnp * pnp; + + if((fp=fopen(fname, "w"))==NULL){ + fprintf(stderr, "JtJ2EPS(): failed to open file %s for writing\n", fname); + exit(1); + } + + /* print EPS preamble */ + time(&tim); + fprintf(fp, "%%!PS-Adobe-2.0 EPSF-2.0\n%%%%Title: %s\n%%%%Creator: sba ver. %s\n", fname, SBA_VERSION); + fprintf(fp, "%%%%CreationDate: %s", ctime(&tim)); + fprintf(fp, "%%%%BoundingBox: 0 0 %d %d\n%%%%Magnification: 1.0000\n%%%%Page: 1 1\n%%%%EndComments\n\n", A4_WIDTH, A4_WIDTH+VERT_OFFSET); + fprintf(fp, "/origstate save def\ngsave\n0 setgray\n"); + if(bbox) fprintf(fp, "0 0 %d %d rectstroke\n", A4_WIDTH, A4_WIDTH+VERT_OFFSET); + + /* move the coordinate system to the upper left corner with axes pointing as shown below: + * +------> y + * | + * | + * | + * v x + */ + fprintf(fp, "%d %d translate\n-90 rotate\n", HORZ_OFFSET, A4_WIDTH+VERT_OFFSET); + /* scale the coordinate system so that the hessian pattern fits in the narrowest page dimension */ + fprintf(fp, "%.4lf %.4lf scale\n", ((double)(A4_WIDTH-HORZ_OFFSET))/nvars, ((double)(A4_WIDTH-HORZ_OFFSET))/nvars); + + /* define dot dimensions */ + fprintf(fp, "/w %g def\n/h %g def\n", DOT_WIDTH, DOT_HEIGHT); + /* define shorthand for rectfill */ + fprintf(fp, "/R { rectfill } bind def\n\n"); + + /* process block row j: [0, ..., 0, U_j, 0, ..., 0, W1j, ..., Wnj] */ + for(j=mcon; j<m; ++j){ + ptr1=U + j*Usz; // set ptr1 to point to U_j + nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero W_ij, i=0...n-1 */ + + i0=(j-mcon)*cnp; + for(ii=0; ii<cnp; ++ii){ + j0=(j-mcon)*cnp; + for(jj=0; jj<cnp; ++jj){ // row ii of U_j + if(denseblocks || (!denseblocks && ptr1[ii*cnp+jj]!=0.0)) + fprintf(fp, "%d %d w h R\n", i0+ii, j0+jj); + } + + for(i=0; i<nnz; ++i){ + /* set ptr2 to point to W_ij, actual row number in rcsubs[i] */ + ptr2=W + idxij->val[rcidxs[i]]*Wsz; + j0=mmcon*cnp+rcsubs[i]*pnp; + for(jj=0; jj<pnp; ++jj){ // row ii of W_ij + if(denseblocks || (!denseblocks && ptr2[ii*pnp+jj]!=0.0)) + fprintf(fp, "%d %d w h R\n", i0+ii, j0+jj); + } + } + } + } + + /* process block row mmcon+i: [W_i1^T, ..., W_im^T, 0, ..., 0, V_i, 0, ..., 0] */ + for(i=0; i<n; ++i){ + nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero W_ij, j=0...m-1 */ + ptr1=V + i*Vsz; // set ptr1 to point to V_i + + i0=cnp*mmcon+i*pnp; + for(ii=0; ii<pnp; ++ii){ + for(j=0; j<nnz; ++j){ + /* set ptr2 to point to W_ij, actual column number in rcsubs[j] */ + if(rcsubs[j]<mcon) continue; /* W_ij is zero */ + + ptr2=W + idxij->val[rcidxs[j]]*Wsz; + + j0=(rcsubs[j]-mcon)*cnp; + for(jj=0; jj<cnp; ++jj){ // row ii of W_ij^T + if(denseblocks || (!denseblocks && ptr2[jj*pnp+ii]!=0.0)) + fprintf(fp, "%d %d w h R\n", i0+ii, j0+jj); + } + } + + j0=cnp*mmcon+i*pnp; + for(jj=0; jj<pnp; ++jj){ // row ii of V_i + if(denseblocks || (!denseblocks && ptr1[ii*pnp+jj]!=0.0)) + fprintf(fp, "%d %d w h R\n", i0+ii, j0+jj); + } + } + } + + fprintf(fp, "grestore\norigstate restore\n\n%%%%Trailer"); + fclose(fp); +} diff --git a/libmoped/libs/sba-1.6/utils/quats.pl b/libmoped/libs/sba-1.6/utils/quats.pl new file mode 100755 index 0000000..b1ccc7e --- /dev/null +++ b/libmoped/libs/sba-1.6/utils/quats.pl @@ -0,0 +1,122 @@ +#!/usr/bin/perl -w + +################################################################################# +## +## Perl functions for manipulating quaternions. Currently includes converters +## between rotation matrices and quaternions. +## Copyright (C) 2008 Manolis Lourakis (lourakis at ics.forth.gr) +## Institute of Computer Science, Foundation for Research & Technology - Hellas +## Heraklion, Crete, Greece. +## +## This program is free software; you can redistribute it and/or modify +## it under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 2 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, +## but WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +################################################################################# + + +$R[0]=3.2471667291e-01; $R[1]=-1.0372666302e-01; $R[2]=-9.4010630341e-01; +$R[3]=2.7245486596e-01; $R[4]=9.6209316145e-01; $R[5]=-1.2045526681e-02; +$R[6]=9.0571928783e-01; $R[7]=-2.5222515354e-01; $R[8]=3.4066852448e-01; + +@q=rotmat2quat(@R); + +printf "Quat from R: %g %g %g %g\n\n", $q[0], $q[1], $q[2], $q[3]; +@Rn=quat2rotmat(@q); +printf "R converted back from quat\n"; +printf "%g %g %g\n", $Rn[0], $Rn[1], $Rn[2]; +printf "%g %g %g\n", $Rn[3], $Rn[4], $Rn[5]; +printf "%g %g %g\n", $Rn[6], $Rn[7], $Rn[8]; + + + +# compute the quaternion corresponding to a rotation matrix; see A8 in Horn's paper +sub rotmat2quat{ + my ($R)=@_; + my ($q, $i, $maxpos, @tmp, $mag); + + # find the maximum of the 4 quantities + $tmp[0]=1.0 + $R[0] + $R[4] + $R[8]; + $tmp[1]=1.0 + $R[0] - $R[4] - $R[8]; + $tmp[2]=1.0 - $R[0] + $R[4] - $R[8]; + $tmp[3]=1.0 - $R[0] - $R[4] + $R[8]; + + for($i=0, $mag=-1.0; $i<4; $i++){ + if($tmp[$i]>$mag){ + $mag=$tmp[$i]; + $maxpos=$i; + } + } + + if($maxpos==0){ + $q[0]=sqrt($tmp[0])*0.5; + $q[1]=($R[7] - $R[5])/(4.0*$q[0]); + $q[2]=($R[2] - $R[6])/(4.0*$q[0]); + $q[3]=($R[3] - $R[1])/(4.0*$q[0]); + } + elsif($maxpos==1){ + $q[1]=sqrt($tmp[1])*0.5; + $q[0]=($R[7] - $R[5])/(4.0*$q[1]); + $q[2]=($R[3] + $R[1])/(4.0*$q[1]); + $q[3]=($R[2] + $R[6])/(4.0*$q[1]); + } + elsif($maxpos==2){ + $q[2]=sqrt($tmp[2])*0.5; + $q[0]=($R[2] - $R[6])/(4.0*$q[2]); + $q[1]=($R[3] + $R[1])/(4.0*$q[2]); + $q[3]=($R[7] + $R[5])/(4.0*$q[2]); + } + elsif($maxpos==3){ + $q[3]=sqrt($tmp[3])*0.5; + $q[0]=($R[3] - $R[1])/(4.0*$q[3]); + $q[1]=($R[2] + $R[6])/(4.0*$q[3]); + $q[2]=($R[7] + $R[5])/(4.0*$q[3]); + } + else{ # should not happen + die "Internal error in rotmat2quat\n"; + } + + # enforce unit length + $mag=$q[0]*$q[0] + $q[1]*$q[1] + $q[2]*$q[2] + $q[3]*$q[3]; + + return @q if($mag==1.0); + + $mag=1.0/sqrt($mag); + $q[0]*=$mag; $q[1]*=$mag; $q[2]*=$mag; $q[3]*=$mag; + + return @q; +} + + +# compute the rotation matrix corresponding to a quaternion; see Horn's paper +sub quat2rotmat{ + my ($q)=@_; + my ($R, $mag); + + # ensure unit length + $mag=$q[0]*$q[0] + $q[1]*$q[1] + $q[2]*$q[2] + $q[3]*$q[3]; + if($mag!=1.0){ + $mag=1.0/sqrt($mag); + $q[0]*=$mag; $q[1]*=$mag; $q[2]*=$mag; $q[3]*=$mag; + } + + $R[0]=$q[0]*$q[0]+$q[1]*$q[1]-$q[2]*$q[2]-$q[3]*$q[3]; + $R[1]=2*($q[1]*$q[2]-$q[0]*$q[3]); + $R[2]=2*($q[1]*$q[3]+$q[0]*$q[2]); + + $R[3]=2*($q[1]*$q[2]+$q[0]*$q[3]); + $R[4]=$q[0]*$q[0]+$q[2]*$q[2]-$q[1]*$q[1]-$q[3]*$q[3]; + $R[5]=2*($q[2]*$q[3]-$q[0]*$q[1]); + + $R[6]=2*($q[1]*$q[3]-$q[0]*$q[2]); + $R[7]=2*($q[2]*$q[3]+$q[0]*$q[1]); + $R[8]=$q[0]*$q[0]+$q[3]*$q[3]-$q[1]*$q[1]-$q[2]*$q[2]; + + return @R; +} diff --git a/libmoped/libs/sba-1.6/utils/reprerr.pl b/libmoped/libs/sba-1.6/utils/reprerr.pl new file mode 100755 index 0000000..92c3442 --- /dev/null +++ b/libmoped/libs/sba-1.6/utils/reprerr.pl @@ -0,0 +1,523 @@ +#!/usr/bin/perl -w + +################################################################################# +## +## Perl script for computing the reprojection error corresponding to a +## given reconstruction. Currently, projective and quaternion-based Euclidean +## reconstructions are supported. More reconstruction types can be added by +## supplying appropriate camera matrix generation routines (i.e. CamMat_Generate) +## Copyright (C) 2005 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. +## +################################################################################# + +use lib '/home/lourakis/sba-src/sba-1.6/utils'; # change this! +use SBAio; + +################################################################################# +# Initializations + +our ($usage, $help); + +$usage="Usage is $0 -e|-i|-I|-p [-r,-s,-P,-h] <cams file> <pts file> [<calib file>]"; +$help="-e specifies a Euclidean reconstruction with fixed intrinsics,\n-i a Euclidean reconstruction with varying intrinsics,\n-I a Euclidean reconstruction with varying intrinsics and lens distortion and -p a projective one.\n" +."-s computes the average *squared* reprojection error; -P prints camera matrices."; +use constant EUCBA => 0; # Euclidean BA, fixed intrinsics +use constant EUCIBA => 1; # Euclidean BA, varying intrinsics +use constant EUCIDBA => 2; # Euclidean BA, varying intrinsics & distortion +use constant PROJBA => 3; # Projective BA +$cnp=$pnp=0; +$camsfile=$ptsfile=$calfile=""; +$CamMat_Generate=\&dont_know; + +################################################################################# +# Basic arguments parsing + +use Getopt::Std; +getopts("eiIrpsPh", \%opt) or die "$usage\n"; +die "$0 help: Compute the average reprojection error for some reconstruction.\n$usage\n$help\n" if($opt{'h'}); + +$i=(defined($opt{'e'})? 1 : 0) + (defined($opt{'i'})? 1 : 0) + + (defined($opt{'I'})? 1 : 0) + (defined($opt{'p'})? 1 : 0); +if($i>1){ + die "$0: Only one of -e, -i, -I, -p can be specified!\n"; +} elsif($i==0){ + die "$0: One of -e, -i, -I, -p should be specified!\n"; +} elsif($opt{'e'}){ + $batype=EUCBA; +} elsif($opt{'i'}){ + $batype=EUCIBA; +} elsif($opt{'I'}){ + $batype=EUCIDBA; +} elsif($opt{'p'}){ + $batype=PROJBA; +} +$squared=$opt{'s'}? 1 : 0; +$printPs=$opt{'P'}? 1 : 0; +$reverse=$opt{'r'}? 1 : 0; # reverse motion: use [R'|-R'*t] instead of [R|t] for the camera matrices +die "$0: -r is meaningful only in combination with one of -e, -i, -I!\n" if($batype!=EUCBA && $batype!=EUCIBA && $batype!=EUCIDBA); + +################################################################################# +# Initializations depending on reconstruction type +if($batype==EUCBA){ + $cnp=7; $pnp=3; + die "$0: Cameras, points, or calibration file is missing!\n$usage" if(@ARGV<3); + die "$0: Too many arguments!\n$usage" if(@ARGV>3); + $camsfile=$ARGV[0]; + $ptsfile=$ARGV[1]; + $calfile=$ARGV[2]; + $CamMat_Generate=($reverse==0)? \&PfromRtK : \&PfromRtRevK; +} +elsif($batype==EUCIBA){ + $cnp=7+5; $pnp=3; + die "$0: Cameras or points file is missing!\n$usage" if(@ARGV<2); + die "$0: Too many arguments!\n$usage" if(@ARGV>2); + $camsfile=$ARGV[0]; + $ptsfile=$ARGV[1]; + $CamMat_Generate=($reverse==0)? \&PfromRtVarK : \&PfromRtRevVarK; +} +elsif($batype==EUCIDBA){ + $cnp=7+5+5; $pnp=3; + die "$0: Cameras or points file is missing!\n$usage" if(@ARGV<2); + die "$0: Too many arguments!\n$usage" if(@ARGV>2); + $camsfile=$ARGV[0]; + $ptsfile=$ARGV[1]; + $CamMat_Generate=($reverse==0)? \&PfromRtVarKD : \&PfromRtRevVarKD; +} +elsif($batype==PROJBA){ + $cnp=12; $pnp=4; + die "$0: Cameras or points file is missing!\n$usage" if(@ARGV<2); + die "$0: Too many arguments!\n$usage" if(@ARGV>2); + $camsfile=$ARGV[0]; + $ptsfile=$ARGV[1]; + $CamMat_Generate=\&nop; +} +else{ + die "Unknown BA type \"$batype\" specified!\n"; +} + +die "$0: Do not know how to handle $pnp parameters per point!\n" if($pnp!=3 && $pnp!=4); + + +################################################################################# +# Main code for computing the reprojection error. + +@camPoses=(); # array of arrays storing each camera's pose; + # Note that in the presence of distortion, camera matrices do not include the intrinsics K! + +$camCal=(); # 3x3 array for storing the camera intrinsic calibration (only when identical for all cameras) + +@camCalibs=(); # array of arrays storing each camera's intrinsics; only used when $batype==EUCIDBA +@camDistorts=(); # array of arrays storing each camera's distortion parameters; only used when $batype==EUCIDBA + + +# read calibration file, if there is one + if(length($calfile)>0){ + $camCal=SBAio::readCalib($calfile); + } + +# read cameras file + $camParms=SBAio::readCameras($camsfile, $cnp); + for($i=0; $i<scalar(@$camParms); $i++){ + if($batype!=EUCIDBA){ + push @camPoses, $CamMat_Generate->($i, $camParms->[$i], $camCal); + } + else{ + ($camPoses[$i], $camCalibs[$i], $camDistorts[$i])=$CamMat_Generate->($i, $camParms->[$i]); + } + } + @$camParms=(); # not needed anymore + + printf "Read %d cameras\n", scalar(@camPoses); + + if($printPs){ # NOTE: K not included in P's in the presence of distortion! + for($i=0; $i<scalar(@camPoses); $i++){ + printf "%g %g %g %g\n", $camPoses[$i]->[0], $camPoses[$i]->[1], $camPoses[$i]->[2], $camPoses[$i]->[3]; + printf "%g %g %g %g\n", $camPoses[$i]->[4], $camPoses[$i]->[5], $camPoses[$i]->[6], $camPoses[$i]->[7]; + printf "%g %g %g %g\n\n", $camPoses[$i]->[8], $camPoses[$i]->[9], $camPoses[$i]->[10], $camPoses[$i]->[11]; + } + } + +# read points file + ($threeDpts, $twoDtrajs, $trajsFrames, $totframes)=SBAio::readPoints($ptsfile, $pnp); + + printf "Read %d 3D points \& trajectories, projecting onto %d image points\n", scalar(@$threeDpts), $totframes; + +# Data file has now been read. Following fragment shows how it can be printed +if(0){ + for($i=0; $i<scalar(@$threeDpts); $i++){ + for($j=0; $j<$pnp; $j++){ + printf "%.6g ", $threeDpts->[$i][$j]; + } + + printf "%d ", $trajsFrames->[$i][0]; + for($j=0; $j<$trajsFrames->[$i][0]; $j++){ + $fr=$trajsFrames->[$i][$j+1]; + if(defined($twoDtrajs->[$i]{$fr})){ + printf "%d %.6g %.6g ", $fr, $twoDtrajs->[$i]{$fr}[0], $twoDtrajs->[$i]{$fr}[1]; + } + } + print "\n"; + } +} + +# compute average, min & max trajectory lengths + $avlen=0; $maxlen=-1; $minlen=99999999; + for($i=0; $i<scalar(@$threeDpts); $i++){ + $avlen+=$trajsFrames->[$i][0]; + $maxlen=$trajsFrames->[$i][0] if($trajsFrames->[$i][0]>$maxlen); + $minlen=$trajsFrames->[$i][0] if($trajsFrames->[$i][0]<$minlen); + } + $avlen/=scalar(@$threeDpts); + printf "Average trajectory length is %g frames [min %d, max %d], S density %.2f%% \n\n", $avlen, $minlen, $maxlen, + density(scalar(@camPoses), scalar(@$threeDpts), $trajsFrames, $twoDtrajs)*100.0; + +# compute reprojection error + unless ($batype==EUCBA || $batype==EUCIBA || $batype==EUCIDBA || $batype==PROJBA){ + die "current implementations of reprError() cannot handle supplied reconstruction data!\n"; + } + + $toterr=0.0; + $totsqerr=0.0; + $totprojs=0; + @error=(); + @sqerror=(); + for($fr=0; $fr<scalar(@camPoses); $fr++){ + $error[$fr]=0.0; + $sqerror[$fr]=0.0; + for($i=$j=0; $i<scalar(@$threeDpts); $i++){ + if(defined($twoDtrajs->[$i]{$fr})){ + if($batype!=EUCIDBA){ + $theerr=&reprErrorNoDistortion($twoDtrajs->[$i]{$fr}, $camPoses[$fr], $threeDpts->[$i], $pnp); + } else{ + $theerr=&reprErrorWithDistortion($twoDtrajs->[$i]{$fr}, $camPoses[$fr], $threeDpts->[$i], $camCalibs[$fr], $camDistorts[$fr]); + } + $theerr=sqrt($theerr) if(!$squared); + $error[$fr]+=$theerr; + $sqerror[$fr]+=$theerr*$theerr; +# printf "@@@ point %d, camera %d: %g\n", $i, $fr, $theerr; + $j++; + } + } + if($j){ + $mean=$error[$fr]/$j; + $sdev=sqrt($sqerror[$fr]/$j - $mean*$mean); + printf "Mean %serror for camera %d [%d projections] is %g, stdev %g\n", $squared? "squared " :"", $fr, $j, $mean, $sdev; + $toterr+=$error[$fr]; + $totsqerr+=$error[$fr]*$error[$fr]; + $totprojs+=$j; + } else{ + printf "No projections for camera %d!\n", $fr; + } + } + + printf STDERR "\nWarning: total number of image projections does not agree with that read with trajectories! [%d != %d]\n", $totprojs, $totframes if($totframes!=$totprojs); + + $mean=$toterr/$totprojs; + printf "\nMean %serror for the whole sequence [%d projections] is %g, stdev %g\n", $squared? "squared " :"", $totprojs, $mean, sqrt($totsqerr/$totprojs-$mean*$mean) if($totprojs); + + + +################################################################################# +# Misc routines + +# measure the density of the points submatrix S +sub density{ + my ($ncams, $npts, $trjfrms, $trjs)=@_; + my ($i, $i2, $i3, $j, $k, @S, $nnz); + + @S=(); # array of hashes keeping track of "connected" frames + for($j=0; $j<$ncams; $j++){ + $S[$j]={}; + } + + for($i=0; $i<$npts; $i++){ + for($i2=1; $i2<=$trjfrms->[$i][0]; $i2++){ + $j=$trjfrms->[$i][$i2]; + for($i3=$i2+1; $i3<=$trjfrms->[$i][0]; $i3++){ + $k=$trjfrms->[$i][$i3]; + $S[$j]{$k}=1; + } + } + } + + for($j=$nnz=0; $j<$ncams; $j++){ + for($k=$j+1; $k<$ncams; $k++){ + $nnz+=2 if(defined($S[$j]{$k}) || defined($S[$k]{$j})); # S is symmetric, both Sjk and Skj are counted + } + } + $nnz+=$ncams; # add diagonal elements (not counted above) + + return $nnz/($ncams*$ncams); +} + +################################################################################# +# Reprojection error calculation routines + +# compute the SQUARED reprojection error |x-xx|^2 with xx=P*X when no distortion is present +sub reprErrorNoDistortion{ + my ($x, $P, $X, $pnp)=@_; + + # error checking + unless (@_==4 && ref($x) eq 'ARRAY' && ref($P) eq 'ARRAY' && ref($X) eq 'ARRAY'){ + die "usage: reprErrorNoDistortion ARRAYREF1 ARRAYREF2 ARRAYREF3 pnp"; + } + + my @xx=(); + my $k; + + # compute the projection in xx + for($k=0; $k<3; $k++){ + $xx[$k]=$P->[$k*4]*$X->[0] + $P->[$k*4+1]*$X->[1] + $P->[$k*4+2]*$X->[2] + $P->[$k*4+3]*(($pnp==4)? $X->[3] : 1.0); + } + $xx[0]/=$xx[2]; + $xx[1]/=$xx[2]; + +# printf "[%g %g -- %g %g]\n", $x->[0], $x->[1], $xx[0], $xx[1]; + + return ($x->[0]-$xx[0])*($x->[0]-$xx[0]) + ($x->[1]-$xx[1])*($x->[1]-$xx[1]); +} + +# compute the SQUARED reprojection error |x-xx|^2 in the presence of distortion +sub reprErrorWithDistortion{ + my ($x, $P, $X, $K, $kc)=@_; + + # error checking + unless (@_==5 && ref($x) eq 'ARRAY' && ref($P) eq 'ARRAY' && ref($X) eq 'ARRAY' && ref($K) eq 'ARRAY' && ref($kc) eq 'ARRAY'){ + die "usage: reprErrorWithDistortion ARRAYREF1 ARRAYREF2 ARRAYREF3 ARRAYREF4 ARRAYREF5"; + } + + my @xx=(); + my @yy=(); + my @dxx=(); + my ($k, $s); + + # compute the projection in xx, P is assumed not to include calibration K! + for($k=0; $k<3; $k++){ + $xx[$k]=$P->[$k*4]*$X->[0] + $P->[$k*4+1]*$X->[1] + $P->[$k*4+2]*$X->[2] + $P->[$k*4+3]; + } + $xx[0]/=$xx[2]; + $xx[1]/=$xx[2]; + + # distort xx into yy + $rsq=$xx[0]*$xx[0] + $xx[1]*$xx[1]; + $rad=1 + $rsq*($kc->[0] + $rsq*($kc->[1] + $rsq*$kc->[4])); + + # radial distortion + $yy[0]=$rad*$xx[0]; + $yy[1]=$rad*$xx[1]; + + # tangential component + $yy[0]+=2*$kc->[2]*$xx[0]*$xx[1] + $kc->[3]*($rsq+2*$xx[0]*$xx[0]); + $yy[1]+=$kc->[2]*($rsq+2*$xx[1]*$xx[1]) + 2*$kc->[3]*$xx[0]*$xx[1]; + + # apply K + $s=1.0/($K->[6]*$yy[0] + $K->[7]*$yy[1] + $K->[8]); + $dxx[0]=($K->[0]*$yy[0] + $K->[1]*$yy[1] + $K->[2])*$s; + $dxx[1]=($K->[3]*$yy[0] + $K->[4]*$yy[1] + $K->[5])*$s; + +# printf "[%g %g -- %g %g]\n", $x->[0], $x->[1], $dxx[0], $dxx[1]; + + return ($x->[0]-$dxx[0])*($x->[0]-$dxx[0]) + ($x->[1]-$dxx[1])*($x->[1]-$dxx[1]); +} + + +################################################################################# +# Camera matrix generation routines + +sub dont_know { + my ($camid, $camparms)=@_; + + die "Don't know how to generate a projection matrix for camera $camid from the supplied camera parameters!\n"; + return $camparms; +} + +# Return as is +sub nop { + my ($camid, $camparms)=@_; + + return $camparms; +} + +# Compute P as K[R|t]. R is specified by the first 4 elements of $camparms, while t corresponds to the last 3 ones +sub PfromRtK { + my ($camid, $camparms, $calparms)=@_; + + my ($x, $y, $z, $w, $xx, $xy, $xz, $xw, $yy, $yz, $yw, $zz, $zw, $ww, $i, $j, $k); + my (@R, @P); + my $mag; + + @R=(); @P=(); # 3x3 & 3x4 resp. +# compute the rotation matrix for q=(x, y, z, w); +# see also http://www.gamedev.net/reference/articles/article1095.asp (but note that q=(w, x, y, z) there!) + + $x=$camparms->[0]; $y=$camparms->[1]; + $z=$camparms->[2]; $w=$camparms->[3]; + + # normalize quaternion + $mag=1.0/sqrt($x*$x + $y*$y + $z*$z + $w*$w); + $x*=$mag; $y*=$mag; $z*=$mag; $w*=$mag; + + $xx=$x*$x; $xy=$x*$y; $xz=$x*$z; $xw=$x*$w; + $yy=$y*$y; $yz=$y*$z; $yw=$y*$w; + $zz=$z*$z; $zw=$z*$w; $ww=$w*$w; + $R[0]=$xx+$yy - ($zz+$ww); $R[1]=2.0*($yz-$xw); $R[2]=2.0*($yw+$xz); + $R[3]=2.0*($yz+$xw); $R[4]=$xx+$zz - ($yy+$ww); $R[5]=2.0*($zw-$xy); + $R[6]=2.0*($yw-$xz); $R[7]=2.0*($zw+$xy); $R[8]=$xx+$ww - ($yy+$zz); + +#print "@R\n\n"; +# compute the matrix-matrix & matrix-vector products + for($i=0; $i<3; $i++){ + for($j=0; $j<3; $j++){ + for($k=0, $sum=0.0; $k<3; $k++){ + $sum+=$calparms->[$i*3+$k]*$R[$k*3+$j]; + } + $P[$i*4+$j]=$sum; + } + for($j=0, $sum=0.0; $j<3; $j++){ + $sum+=$calparms->[$i*3+$j]*$camparms->[4+$j]; + } + $P[$i*4+3]=$sum; + } + + return [@P]; +} + +# As above but compute P as K[R'|-R'*t] +sub PfromRtRevK{ + my ($camid, $camparms, $calparms)=@_; + my ($mag, $tmp, $q, $t); + + # normalize quaternion + $mag=1.0/sqrt($camparms->[0]*$camparms->[0] + $camparms->[1]*$camparms->[1] + + $camparms->[2]*$camparms->[2] + $camparms->[3]*$camparms->[3]); + $camparms->[0]*=$mag; $camparms->[1]*=$mag; $camparms->[2]*=$mag; $camparms->[3]*=$mag; + + $camparms->[0]=-$camparms->[0]; # opposite angle + + # compute -R'*t using the quaternion: -q*(0, t)*qc, qc is q's conjugate + # note that the quat multiplications below are adapted to using q & t and are not general-purpose! + $q[0]=$camparms->[0]; $q[1]=$camparms->[1]; $q[2]=$camparms->[2]; $q[3]=$camparms->[3]; + $t[0]=$camparms->[4]; $t[1]=$camparms->[5]; $t[2]=$camparms->[6]; + + # compute tmp as -q*t + $tmp[0]=-( - $q[1]*$t[0] - $q[2]*$t[1] - $q[3]*$t[2]); + $tmp[1]=-($q[0]*$t[0] + $q[2]*$t[2] - $q[3]*$t[1]); + $tmp[2]=-($q[0]*$t[1] + $q[3]*$t[0] - $q[1]*$t[2]); + $tmp[3]=-($q[0]*$t[2] + $q[1]*$t[1] - $q[2]*$t[0]); + + # compute tmp*qc + #always zero: $tmp[0]*$q[0] + $tmp[1]*$q[1] + $tmp[2]*$q[2] + $tmp[3]*$q[3]; + $camparms->[4]=-$tmp[0]*$q[1] + $tmp[1]*$q[0] - $tmp[2]*$q[3] + $tmp[3]*$q[2]; + $camparms->[5]=-$tmp[0]*$q[2] + $tmp[2]*$q[0] - $tmp[3]*$q[1] + $tmp[1]*$q[3]; + $camparms->[6]=-$tmp[0]*$q[3] + $tmp[3]*$q[0] - $tmp[1]*$q[2] + $tmp[2]*$q[1]; +printf "%g %g %g %g %g %g %g\n", $camparms->[0], $camparms->[1], $camparms->[2], $camparms->[3], $camparms->[4], $camparms->[5], $camparms->[6]; + + return &PfromRtK($camid, $camparms, $calparms); +} + +# Compute P as K[R|t]. K is specified by the first 5 elements of $camparms, while R and t correspond to the next 4 & 3 elements, respectively +sub PfromRtVarK { + my ($camid, $camparms)=@_; + my (@K, @poseparms, $size, $i); + + @K=(); @poseparms=(); + # setup the intrinsics matrix from the 5 first elements + $K[0]=$camparms->[0]; $K[1]=$camparms->[4]; $K[2]=$camparms->[1]; + $K[3]=0.0; $K[4]=$camparms->[3]*$camparms->[0]; $K[5]=$camparms->[2]; + $K[6]=0.0; $K[7]=0.0; $K[8]=1.0; + + $size=scalar(@$camparms); + for($i=5; $i<$size; $i++){ + $poseparms[$i-5]=$camparms->[$i]; + } + + return &PfromRtK($camid, [@poseparms], [@K]); +} + +# As above but compute P as K[R'|-R'*t] +sub PfromRtRevVarK{ + my ($camid, $camparms)=@_; + my (@K, @poseparms, $size, $i); + + @K=(); @poseparms=(); + # setup the intrinsics matrix from the 5 first elements + $K[0]=$camparms->[0]; $K[1]=$camparms->[4]; $K[2]=$camparms->[1]; + $K[3]=0.0; $K[4]=$camparms->[3]*$camparms->[0]; $K[5]=$camparms->[2]; + $K[6]=0.0; $K[7]=0.0; $K[8]=1.0; + + $size=scalar(@$camparms); + for($i=5; $i<$size; $i++){ + $poseparms[$i-5]=$camparms->[$i]; + } + + return &PfromRtRevK($camid, [@poseparms], [@K]); +} + +# Compute P as [R|t]. K is specified by the first 5 elements of $camparms, distortion from the next 5. +# Parameters for R and t correspond to the next 4 & 3 elements, respectively +sub PfromRtVarKD { + my ($camid, $camparms)=@_; + my (@K, @I3, @poseparms, @distparms, $size, $i); + + @K=(); @I3=(); @poseparms=(); @distparms=(); @P=(); + # setup the intrinsics matrix from the 5 first elements + $K[0]=$camparms->[0]; $K[1]=$camparms->[4]; $K[2]=$camparms->[1]; + $K[3]=0.0; $K[4]=$camparms->[3]*$camparms->[0]; $K[5]=$camparms->[2]; + $K[6]=0.0; $K[7]=0.0; $K[8]=1.0; + + for($i=5; $i<10; $i++){ + $distparms[$i-5]=$camparms->[$i]; + } + $size=scalar(@$camparms); + for($i=10; $i<$size; $i++){ + $poseparms[$i-10]=$camparms->[$i]; + } + + $I3[0]=1.0; $I3[1]=0.0; $I3[2]=0.0; + $I3[3]=0.0; $I3[4]=1.0; $I3[5]=0.0; + $I3[6]=0.0; $I3[7]=0.0; $I3[8]=1.0; + + $P=&PfromRtK($camid, [@poseparms], [@I3]); + + return ($P, [@K], [@distparms]); +} + +# As above but compute P as K[R'|-R'*t] +sub PfromRtRevVarKD { + my ($camid, $camparms)=@_; + my (@K, @I3, @poseparms, @distparms, $size, $i, @P); + + @K=(); @I3=(); @poseparms=(); @distparms=(); @P=(); + # setup the intrinsics matrix from the 5 first elements + $K[0]=$camparms->[0]; $K[1]=$camparms->[4]; $K[2]=$camparms->[1]; + $K[3]=0.0; $K[4]=$camparms->[3]*$camparms->[0]; $K[5]=$camparms->[2]; + $K[6]=0.0; $K[7]=0.0; $K[8]=1.0; + + for($i=5; $i<10; $i++){ + $distparms[$i-5]=$camparms->[$i]; + } + $size=scalar(@$camparms); + for($i=10; $i<$size; $i++){ + $poseparms[$i-10]=$camparms->[$i]; + } + + $I3[0]=1.0; $I3[1]=0.0; $I3[2]=0.0; + $I3[3]=0.0; $I3[4]=1.0; $I3[5]=0.0; + $I3[6]=0.0; $I3[7]=0.0; $I3[8]=1.0; + + $P=&PfromRtRevK($camid, [@poseparms], [@I3]); + + return ($P, [@K], [@distparms]); +} |