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