diff options
Diffstat (limited to 'extern/libmv/third_party/ceres')
160 files changed, 25407 insertions, 0 deletions
diff --git a/extern/libmv/third_party/ceres/CMakeLists.txt b/extern/libmv/third_party/ceres/CMakeLists.txt new file mode 100644 index 00000000000..5207bddec12 --- /dev/null +++ b/extern/libmv/third_party/ceres/CMakeLists.txt @@ -0,0 +1,218 @@ +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +# +# The Original Code is Copyright (C) 2012, Blender Foundation +# All rights reserved. +# +# Contributor(s): Blender Foundation, +# Sergey Sharybin +# +# ***** END GPL LICENSE BLOCK ***** + +# NOTE: This file is automatically generated by bundle.sh script +# If you're doing changes in this file, please update template +# in that script too + +set(INC + . + ../../../Eigen3 + include + internal + ../gflags +) + +set(INC_SYS +) + +set(SRC + internal/ceres/block_evaluate_preparer.cc + internal/ceres/block_jacobian_writer.cc + internal/ceres/block_jacobi_preconditioner.cc + internal/ceres/block_random_access_dense_matrix.cc + internal/ceres/block_random_access_matrix.cc + internal/ceres/block_random_access_sparse_matrix.cc + internal/ceres/block_sparse_matrix.cc + internal/ceres/block_structure.cc + internal/ceres/canonical_views_clustering.cc + internal/ceres/cgnr_solver.cc + internal/ceres/compressed_row_jacobian_writer.cc + internal/ceres/compressed_row_sparse_matrix.cc + internal/ceres/conditioned_cost_function.cc + internal/ceres/conjugate_gradients_solver.cc + internal/ceres/corrector.cc + internal/ceres/dense_qr_solver.cc + internal/ceres/dense_sparse_matrix.cc + internal/ceres/detect_structure.cc + internal/ceres/evaluator.cc + internal/ceres/file.cc + internal/ceres/generated/schur_eliminator_2_2_2.cc + internal/ceres/generated/schur_eliminator_2_2_3.cc + internal/ceres/generated/schur_eliminator_2_2_4.cc + internal/ceres/generated/schur_eliminator_2_2_d.cc + internal/ceres/generated/schur_eliminator_2_3_3.cc + internal/ceres/generated/schur_eliminator_2_3_4.cc + internal/ceres/generated/schur_eliminator_2_3_9.cc + internal/ceres/generated/schur_eliminator_2_3_d.cc + internal/ceres/generated/schur_eliminator_2_4_3.cc + internal/ceres/generated/schur_eliminator_2_4_4.cc + internal/ceres/generated/schur_eliminator_2_4_d.cc + internal/ceres/generated/schur_eliminator_4_4_2.cc + internal/ceres/generated/schur_eliminator_4_4_3.cc + internal/ceres/generated/schur_eliminator_4_4_4.cc + internal/ceres/generated/schur_eliminator_4_4_d.cc + internal/ceres/generated/schur_eliminator_d_d_d.cc + internal/ceres/gradient_checking_cost_function.cc + internal/ceres/implicit_schur_complement.cc + internal/ceres/iterative_schur_complement_solver.cc + internal/ceres/levenberg_marquardt.cc + internal/ceres/linear_least_squares_problems.cc + internal/ceres/linear_operator.cc + internal/ceres/linear_solver.cc + internal/ceres/local_parameterization.cc + internal/ceres/loss_function.cc + internal/ceres/normal_prior.cc + internal/ceres/partitioned_matrix_view.cc + internal/ceres/problem.cc + internal/ceres/problem_impl.cc + internal/ceres/program.cc + internal/ceres/residual_block.cc + internal/ceres/residual_block_utils.cc + internal/ceres/runtime_numeric_diff_cost_function.cc + internal/ceres/schur_complement_solver.cc + internal/ceres/schur_eliminator.cc + internal/ceres/schur_ordering.cc + internal/ceres/scratch_evaluate_preparer.cc + internal/ceres/solver.cc + internal/ceres/solver_impl.cc + internal/ceres/sparse_matrix.cc + internal/ceres/sparse_normal_cholesky_solver.cc + internal/ceres/split.cc + internal/ceres/stringprintf.cc + internal/ceres/suitesparse.cc + internal/ceres/triplet_sparse_matrix.cc + internal/ceres/types.cc + internal/ceres/visibility_based_preconditioner.cc + internal/ceres/visibility.cc + + include/ceres/autodiff_cost_function.h + include/ceres/ceres.h + include/ceres/conditioned_cost_function.h + include/ceres/cost_function.h + include/ceres/internal/autodiff.h + include/ceres/internal/eigen.h + include/ceres/internal/fixed_array.h + include/ceres/internal/macros.h + include/ceres/internal/manual_constructor.h + include/ceres/internal/port.h + include/ceres/internal/scoped_ptr.h + include/ceres/iteration_callback.h + include/ceres/jet.h + include/ceres/local_parameterization.h + include/ceres/loss_function.h + include/ceres/normal_prior.h + include/ceres/numeric_diff_cost_function.h + include/ceres/problem.h + include/ceres/rotation.h + include/ceres/sized_cost_function.h + include/ceres/solver.h + include/ceres/types.h + internal/ceres/block_evaluate_preparer.h + internal/ceres/block_jacobian_writer.h + internal/ceres/block_jacobi_preconditioner.h + internal/ceres/block_random_access_dense_matrix.h + internal/ceres/block_random_access_matrix.h + internal/ceres/block_random_access_sparse_matrix.h + internal/ceres/block_sparse_matrix.h + internal/ceres/block_structure.h + internal/ceres/canonical_views_clustering.h + internal/ceres/casts.h + internal/ceres/cgnr_linear_operator.h + internal/ceres/cgnr_solver.h + internal/ceres/collections_port.h + internal/ceres/compressed_row_jacobian_writer.h + internal/ceres/compressed_row_sparse_matrix.h + internal/ceres/conjugate_gradients_solver.h + internal/ceres/corrector.h + internal/ceres/dense_jacobian_writer.h + internal/ceres/dense_qr_solver.h + internal/ceres/dense_sparse_matrix.h + internal/ceres/detect_structure.h + internal/ceres/evaluator.h + internal/ceres/file.h + internal/ceres/gradient_checking_cost_function.h + internal/ceres/graph_algorithms.h + internal/ceres/graph.h + internal/ceres/implicit_schur_complement.h + internal/ceres/integral_types.h + internal/ceres/iterative_schur_complement_solver.h + internal/ceres/levenberg_marquardt.h + internal/ceres/linear_least_squares_problems.h + internal/ceres/linear_operator.h + internal/ceres/linear_solver.h + internal/ceres/map_util.h + internal/ceres/matrix_proto.h + internal/ceres/minimizer.h + internal/ceres/mutex.h + internal/ceres/parameter_block.h + internal/ceres/partitioned_matrix_view.h + internal/ceres/problem_impl.h + internal/ceres/program_evaluator.h + internal/ceres/program.h + internal/ceres/random.h + internal/ceres/residual_block.h + internal/ceres/residual_block_utils.h + internal/ceres/runtime_numeric_diff_cost_function.h + internal/ceres/schur_complement_solver.h + internal/ceres/schur_eliminator.h + internal/ceres/schur_eliminator_impl.h + internal/ceres/schur_ordering.h + internal/ceres/scratch_evaluate_preparer.h + internal/ceres/solver_impl.h + internal/ceres/sparse_matrix.h + internal/ceres/sparse_normal_cholesky_solver.h + internal/ceres/stl_util.h + internal/ceres/stringprintf.h + internal/ceres/suitesparse.h + internal/ceres/triplet_sparse_matrix.h + internal/ceres/visibility_based_preconditioner.h + internal/ceres/visibility.h +) + +if(WIN32) + list(APPEND INC + ../glog/src/windows + ) + + if(NOT MINGW) + list(APPEND INC + third_party/msinttypes + ) + endif() +else() + list(APPEND INC + ../glog/src + ) +endif() + +add_definitions( + -DCERES_HAVE_PTHREAD + -D"CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {" + -D"CERES_HASH_NAMESPACE_END=}}" + -DCERES_NO_SUITESPARSE + -DCERES_DONT_HAVE_PROTOCOL_BUFFERS +) + +blender_add_lib(extern_ceres "${SRC}" "${INC}" "${INC_SYS}") diff --git a/extern/libmv/third_party/ceres/ChangeLog b/extern/libmv/third_party/ceres/ChangeLog new file mode 100644 index 00000000000..6e919658f13 --- /dev/null +++ b/extern/libmv/third_party/ceres/ChangeLog @@ -0,0 +1,324 @@ +commit ca72152362ae1f4b9928c012e74b4d49d094a4ca +Merge: d297f8d 0a04199 +Author: Keir Mierle <mierle@gmail.com> +Date: Wed May 9 13:10:59 2012 -0700 + + Merge branch 'master' into windows + +commit 0a04199ef279cc9ea97f665fed8e7fae717813c3 +Merge: fdeb577 f2571f1 +Author: Keir Mierle <mierle@gmail.com> +Date: Wed May 9 12:54:56 2012 -0700 + + Merge branch 'master' of https://code.google.com/p/ceres-solver + +commit fdeb5772cc5eeebca4d776d220d80cc91b6d0f74 +Author: Keir Mierle <mierle@gmail.com> +Date: Wed May 9 07:38:07 2012 -0700 + + Support varying numbers of residuals in autodiff. + + This commit modifies the only function in autodiff that takes a + templated number of outputs (i.e. residuals) and makes that + template parameter a normal parameter. With that change, it + is a trivial matter to support a dynamic number of residuals. + + The API for dynamic residuals is to pass a fake number of + residuals as the second template argument to + AutoDiffCostFunction, and to pass the real number of + parameters as a second constructor argument. + +commit da3e0563cc12e08e7b3e0fbf11d9cc8cfe9658aa +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed May 9 11:57:47 2012 -0700 + + Typo corrections in the documentation from Bing + +commit aa9526d8e8fb34c23d63e3af5bf9239b0c4ea603 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue May 8 21:22:09 2012 -0700 + + Share search paths across various library searches. + Fix typos in glog search. + Split the error messages for include and lib. + Enable building of tests by default. + Made building on homebrew installations a bit better. + Remove temporary variables for glog and gflags. + +commit f2571f186850ed3dd316236ac4be488979df7d30 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed May 9 11:57:47 2012 -0700 + + Typo corrections in the documentation from Bing + +commit 8f7f11ff7d07737435428a2620c52419cf99f98e +Merge: e6c17c4 eaccbb3 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed May 9 11:34:15 2012 -0700 + + Merge branch 'master' of https://code.google.com/p/ceres-solver + +commit e6c17c4c9d9307218f6f739cea39bc2d87733d4d +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue May 8 21:22:09 2012 -0700 + + Share search paths across various library searches. + Fix typos in glog search. + Split the error messages for include and lib. + Enable building of tests by default. + Made building on homebrew installations a bit better. + Remove temporary variables for glog and gflags. + +commit eaccbb345614c0d24c5e21fa931f470cfda874df +Author: Keir Mierle <mierle@gmail.com> +Date: Wed May 9 05:31:29 2012 -0700 + + Remove unused template parameter from VariadicEvaluate. + +commit 82f4b88c34b0b2cf85064e5fc20e374e978b2e3b +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 21:05:28 2012 -0700 + + Extend support writing linear least squares problems to disk. + + 1. Make the mechanism for writing problems to disk, generic and + controllable using an enum DumpType visible in the API. + + 2. Instead of single file containing protocol buffers, now matrices can + be written in a matlab/octave friendly format. This is now the default. + + 3. The support for writing problems to disk is moved into + linear_least_squares_problem.cc/h + + 4. SparseMatrix now has a ToTextFile virtual method which is + implemented by each of its subclasses to write a (i,j,s) triplets. + + 5. Minor changes to simple_bundle_adjuster to enable logging at startup. + +commit d297f8d3d3f5025c24752f0f4c1ec2469a769f99 +Merge: 7e74d81 f8bd7fa +Author: Keir Mierle <mierle@gmail.com> +Date: Tue May 8 05:39:56 2012 -0700 + + Merge branch 'master' into windows + +commit f8bd7fa9aa9dbf64b6165606630287cf8cf21194 +Author: Keir Mierle <mierle@gmail.com> +Date: Tue May 8 05:39:32 2012 -0700 + + Small tweaks to the block jacobi preconditioner. + +commit 7e74d81ad57a159f14110eb5348b3bc7990b8bd4 +Merge: ecd7c8d e2a6cdc +Author: Keir Mierle <mierle@gmail.com> +Date: Mon May 7 07:02:49 2012 -0700 + + Merge branch 'master' into windows + +commit e2a6cdc0816af9d0c77933f5017f137da3d52a35 +Author: Keir Mierle <mierle@gmail.com> +Date: Mon May 7 06:39:56 2012 -0700 + + Address some of the comments on CGNR patch + + - Rename BlockDiagonalPreconditioner to BlockJacobiPreconditioner + - Include the diagonal in the block jacobi preconditioner. + - Better flag help for eta. + - Enable test for CGNR + - Rename CONJUGATE_GRADIENTS to CGNR. + - etc. + +commit 1b95dc580aa5d89be021c0915e26df83f18013bb +Merge: 211812a 7646039 +Author: Keir Mierle <mierle@gmail.com> +Date: Mon May 7 04:34:10 2012 -0700 + + Merge branch 'master' of https://code.google.com/p/ceres-solver + +commit 211812a57360d2011cbcfd115cd55e0eb73600db +Author: Keir Mierle <mierle@gmail.com> +Date: Mon May 7 04:33:50 2012 -0700 + + Better error handling in bundle_adjuster.cc + +commit 7646039ad9672b267495f5b31925473ad3022ac8 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 22:02:19 2012 -0700 + + Kashif's corrections to the docs + +commit 0d2d34148d10c5c7e924b3ca82ad2b237573ef64 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 21:16:03 2012 -0700 + + glog minimum version requirements + + Building Ceres requires version 0.3.1 or better of glog. + Fedora 16 ships with a busted version 0.3. + + issue 15 contains the gory details. + + Added a note to the build documentation to this effect. + +commit 39efc5ec4b64b8f5a2c5a3dbacdbc45421221547 +Author: Keir Mierle <mierle@gmail.com> +Date: Sun May 6 16:09:52 2012 -0700 + + Fix tests broken by the CGNR change. + +commit 3faa08b7f7c4ac73661c6a15a6824c12080dfcb1 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 16:08:22 2012 -0700 + + Formatting fixed based on Keir's comments and extended the tests + +commit 4f21c68409bc478c431a9b6aedf9e5cfdf11d2f3 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 15:33:47 2012 -0700 + + Fix the struct weak ordering used by independent set ordering, tests for it + +commit 887b156b917ccd4c172484452b059d33ea45f4f0 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 15:14:47 2012 -0700 + + fix he degree ordering routine + +commit ecd7c8df2af19404dc394b36bbe96e9db3bce840 +Author: Keir Mierle <mierle@gmail.com> +Date: Sun May 6 00:09:41 2012 -0700 + + First step towards windows compatibilty + + This adds some small changes to Ceres to make it mostly + compile on Windows. There are still issues with the + hash map use in schur_ordering.cc but I will fix those + shortly. + +commit f7898fba1b92f0e996571b5bfa22a37f5e3644de +Author: Keir Mierle <mierle@gmail.com> +Date: Sat May 5 20:55:08 2012 -0700 + + Add a general sparse iterative solver: CGNR + + This adds a new LinearOperator which implements symmetric + products of a matrix, and a new CGNR solver to leverage + CG to directly solve the normal equations. This also + includes a block diagonal preconditioner. In experiments + on problem-16, the non-preconditioned version is about + 1/5 the speed of SPARSE_SCHUR, and the preconditioned + version using block cholesky is about 20% slower than + SPARSE_SCHUR. + +commit 0a359d6198d257776a8831c3eb98f64ee91cf836 +Author: Keir Mierle <mierle@gmail.com> +Date: Sat May 5 20:33:46 2012 -0700 + + Comment formatting. + +commit db4ec9312bb2f1ca7b2337812f6bad6cdd75b227 +Author: Keir Mierle <mierle@gmail.com> +Date: Sat May 5 20:33:16 2012 -0700 + + Comment formatting + +commit f10163aaf3e57f52551bcd60bbdae873890a49dd +Author: Keir Mierle <mierle@gmail.com> +Date: Fri May 4 21:33:53 2012 -0700 + + Warn about disabled schur specializations. + + This commit brought to you from 30,000ft. + +commit ad7b2b4aaf3ccc51f2b854febd53a9df54686cfe +Author: Keir Mierle <mierle@gmail.com> +Date: Fri May 4 20:15:28 2012 -0700 + + Add vim swapfiles to .gitignore + +commit 6447219826bf6e47b0c99d9ff0eaf5e2ba573d79 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 21:53:07 2012 -0700 + + 1. Changes the tutorial to refer to BriefReport. + 2. Some of the enums have commas at the end. + 3. Fix a bug in the default value of circle_fit.cc in the examples. + +commit 30c5f93c7f88dec49f76168663372772e06f17f5 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 10:44:43 2012 -0700 + + Rework the glog and gtest path checking to be consistent with the rest of the file and disable the dashboard support enabled by the earlier ctesting related patch. + +commit f10b033eb4aca77919987bc551d16d8a88b10110 +Merge: cc38774 e0a52a9 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 08:45:20 2012 -0700 + + Merge branch 'ctest' + +commit e0a52a993394e73bc7f7db8d520728926feab83e +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 08:43:34 2012 -0700 + + Arnaus Gelas' patch to add better path searching for gflags and glog + +commit a9b8e815e1c026599734510399b10f4cf014c9cd +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 08:41:52 2012 -0700 + + Arnaus Gelas' patch to add .gitignore + +commit a0cefc3347c32b2065053bbaff4f34d11529d931 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 08:38:33 2012 -0700 + + Arnaus Gelas' patch to move to Ctest + +commit cc38774d74e287704915282425fbd16818a72ec3 +Author: Keir Mierle <mierle@gmail.com> +Date: Thu May 3 01:27:50 2012 -0700 + + Clarify ProgramEvaluator comments. + +commit 017c9530df557863f78212fb5ccd02814baa9fa8 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed May 2 08:21:59 2012 -0700 + + Mac OS X build instructions are much simpler, as homebrew takes care of gflags when glog is brought in. Also CMAKE does not need any flags to do the default thing + +commit 92d5ab5f8ae6fe355c30b606a5f230415ee0494b +Author: Keir Mierle <mierle@gmail.com> +Date: Tue May 1 18:33:08 2012 -0700 + + Link BLAS explicitly on non-Mac platforms + + Fixes issue #3. + +commit df3e54eb4a6b001b7f0560a2da73a5bd7f18615e +Author: Keir Mierle <mierle@gmail.com> +Date: Tue May 1 18:22:51 2012 -0700 + + Fix link order of CHOLMOD + + This was working by accident due to dynamic linking. Fixes issue #2. + +commit f477a3835329e2b48eb20c34c631a480b0f0d5bf +Author: Keir Mierle <mierle@gmail.com> +Date: Tue May 1 18:10:48 2012 -0700 + + Fix Eigen search paths + + Fixes issue #1 on http://code.google.com/p/ceres-solver. + +commit 17fbc8ebb894c1d22bb3b0b02ea1394b580120f8 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue May 1 00:21:19 2012 -0700 + + Minor changes to the documentation. Formatting, and typos. + +commit 8ebb0730388045570f22b89fe8672c860cd2ad1b +Author: Keir Mierle <mierle@gmail.com> +Date: Mon Apr 30 23:09:08 2012 -0700 + + Initial commit of Ceres Solver. diff --git a/extern/libmv/third_party/ceres/LICENSE b/extern/libmv/third_party/ceres/LICENSE new file mode 100644 index 00000000000..2e3ead5ed45 --- /dev/null +++ b/extern/libmv/third_party/ceres/LICENSE @@ -0,0 +1,27 @@ +Ceres Solver - A fast non-linear least squares minimizer +Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +http://code.google.com/p/ceres-solver/ + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +* Neither the name of Google Inc. nor the names of its contributors may be + used to endorse or promote products derived from this software without + specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/extern/libmv/third_party/ceres/README b/extern/libmv/third_party/ceres/README new file mode 100644 index 00000000000..8dd8ccf91a1 --- /dev/null +++ b/extern/libmv/third_party/ceres/README @@ -0,0 +1,3 @@ +Ceres Solver - A non-linear least squares minimizer +================================================== +Please see ceres.pdf in docs/ for a tutorial and reference. diff --git a/extern/libmv/third_party/ceres/SConscript b/extern/libmv/third_party/ceres/SConscript new file mode 100644 index 00000000000..d8b2b8520d7 --- /dev/null +++ b/extern/libmv/third_party/ceres/SConscript @@ -0,0 +1,34 @@ +#!/usr/bin/python + +# NOTE: This file is automatically generated by bundle.sh script +# If you're doing changes in this file, please update template +# in that script too + +import sys +import os + +Import('env') + +src = [] +defs = [] + +src += env.Glob('internal/ceres/*.cc') +src += env.Glob('internal/ceres/generated/*.cc') + +defs.append('CERES_HAVE_PTHREAD') +defs.append('CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {') +defs.append('CERES_HASH_NAMESPACE_END=}}') +defs.append('CERES_NO_SUITESPARSE') +defs.append('CERES_DONT_HAVE_PROTOCOL_BUFFERS') + +incs = '. ../../ ../../../Eigen3 ./include ./internal ../gflags' + +if env['OURPLATFORM'] in ('win32-vc', 'win32-mingw', 'linuxcross', 'win64-vc', 'win64-mingw'): + if env['OURPLATFORM'] in ('win32-vc', 'win64-vc'): + incs += ' ../msinttypes' + + incs += ' ../glog/src/windows' +else: + incs += ' ../glog/src' + +env.BlenderLib ( libname = 'extern_ceres', sources=src, includes=Split(incs), defines=defs, libtype=['extern', 'player'], priority=[20,137]) diff --git a/extern/libmv/third_party/ceres/bundle.sh b/extern/libmv/third_party/ceres/bundle.sh new file mode 100644 index 00000000000..f54342180db --- /dev/null +++ b/extern/libmv/third_party/ceres/bundle.sh @@ -0,0 +1,185 @@ +#!/bin/sh + +if [ "x$1" = "x--i-really-know-what-im-doing" ] ; then + echo Proceeding as requested by command line ... +else + echo "*** Please run again with --i-really-know-what-im-doing ..." + exit 1 +fi + +repo="https://code.google.com/p/ceres-solver/" +branch="windows" +tmp=`mktemp -d` + +GIT="git --git-dir $tmp/ceres/.git --work-tree $tmp/ceres" + +git clone $repo $tmp/ceres + +if [ $branch != "master" ]; then + $GIT checkout -t remotes/origin/$branch +fi + +$GIT log -n 50 > ChangeLog + +for p in `cat ./patches/series`; do + echo "Applying patch $p..." + cat ./patches/$p | patch -d $tmp/ceres -p1 +done + +find include -type f -not -iwholename '*.svn*' -exec rm -rf {} \; +find internal -type f -not -iwholename '*.svn*' -exec rm -rf {} \; + +cat "files.txt" | while read f; do + mkdir -p `dirname $f` + cp $tmp/ceres/$f $f +done + +rm -rf $tmp + +sources=`find ./include ./internal -type f -iname '*.cc' -or -iname '*.cpp' -or -iname '*.c' | sed -r 's/^\.\//\t/' | sort -d` +headers=`find ./include ./internal -type f -iname '*.h' | sed -r 's/^\.\//\t/' | sort -d` + +src_dir=`find ./internal -type f -iname '*.cc' -exec dirname {} \; -or -iname '*.cpp' -exec dirname {} \; -or -iname '*.c' -exec dirname {} \; | sed -r 's/^\.\//\t/' | sort -d | uniq` +src="" +for x in $src_dir $src_third_dir; do + t="" + + if test `echo "$x" | grep -c glog ` -eq 1; then + continue; + fi + + if stat $x/*.cpp > /dev/null 2>&1; then + t="src += env.Glob('`echo $x'/*.cpp'`')" + fi + + if stat $x/*.c > /dev/null 2>&1; then + if [ -z "$t" ]; then + t="src += env.Glob('`echo $x'/*.c'`')" + else + t="$t + env.Glob('`echo $x'/*.c'`')" + fi + fi + + if stat $x/*.cc > /dev/null 2>&1; then + if [ -z "$t" ]; then + t="src += env.Glob('`echo $x'/*.cc'`')" + else + t="$t + env.Glob('`echo $x'/*.cc'`')" + fi + fi + + if [ -z "$src" ]; then + src=$t + else + src=`echo "$src\n$t"` + fi +done + +cat > CMakeLists.txt << EOF +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +# +# The Original Code is Copyright (C) 2012, Blender Foundation +# All rights reserved. +# +# Contributor(s): Blender Foundation, +# Sergey Sharybin +# +# ***** END GPL LICENSE BLOCK ***** + +# NOTE: This file is automatically generated by bundle.sh script +# If you're doing changes in this file, please update template +# in that script too + +set(INC + . + ../../../Eigen3 + include + internal + ../gflags +) + +set(INC_SYS +) + +set(SRC +${sources} + +${headers} +) + +if(WIN32) + list(APPEND INC + ../glog/src/windows + ) + + if(NOT MINGW) + list(APPEND INC + third_party/msinttypes + ) + endif() +else() + list(APPEND INC + ../glog/src + ) +endif() + +add_definitions( + -DCERES_HAVE_PTHREAD + -D"CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {" + -D"CERES_HASH_NAMESPACE_END=}}" + -DCERES_NO_SUITESPARSE + -DCERES_DONT_HAVE_PROTOCOL_BUFFERS +) + +blender_add_lib(extern_ceres "\${SRC}" "\${INC}" "\${INC_SYS}") +EOF + +cat > SConscript << EOF +#!/usr/bin/python + +# NOTE: This file is automatically generated by bundle.sh script +# If you're doing changes in this file, please update template +# in that script too + +import sys +import os + +Import('env') + +src = [] +defs = [] + +$src + +defs.append('CERES_HAVE_PTHREAD') +defs.append('CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {') +defs.append('CERES_HASH_NAMESPACE_END=}}') +defs.append('CERES_NO_SUITESPARSE') +defs.append('CERES_DONT_HAVE_PROTOCOL_BUFFERS') + +incs = '. ../../ ../../../Eigen3 ./include ./internal ../gflags' + +if env['OURPLATFORM'] in ('win32-vc', 'win32-mingw', 'linuxcross', 'win64-vc', 'win64-mingw'): + if env['OURPLATFORM'] in ('win32-vc', 'win64-vc'): + incs += ' ../msinttypes' + + incs += ' ../glog/src/windows' +else: + incs += ' ../glog/src' + +env.BlenderLib ( libname = 'extern_ceres', sources=src, includes=Split(incs), defines=defs, libtype=['extern', 'player'], priority=[20,137]) +EOF diff --git a/extern/libmv/third_party/ceres/files.txt b/extern/libmv/third_party/ceres/files.txt new file mode 100644 index 00000000000..e9d7f585260 --- /dev/null +++ b/extern/libmv/third_party/ceres/files.txt @@ -0,0 +1,150 @@ +include/ceres/autodiff_cost_function.h +include/ceres/ceres.h +include/ceres/conditioned_cost_function.h +include/ceres/cost_function.h +include/ceres/internal/autodiff.h +include/ceres/internal/eigen.h +include/ceres/internal/fixed_array.h +include/ceres/internal/macros.h +include/ceres/internal/manual_constructor.h +include/ceres/internal/port.h +include/ceres/internal/scoped_ptr.h +include/ceres/iteration_callback.h +include/ceres/jet.h +include/ceres/local_parameterization.h +include/ceres/loss_function.h +include/ceres/normal_prior.h +include/ceres/numeric_diff_cost_function.h +include/ceres/problem.h +include/ceres/rotation.h +include/ceres/sized_cost_function.h +include/ceres/solver.h +include/ceres/types.h +internal/ceres/block_evaluate_preparer.cc +internal/ceres/block_evaluate_preparer.h +internal/ceres/block_jacobian_writer.cc +internal/ceres/block_jacobian_writer.h +internal/ceres/block_jacobi_preconditioner.cc +internal/ceres/block_jacobi_preconditioner.h +internal/ceres/block_random_access_dense_matrix.cc +internal/ceres/block_random_access_dense_matrix.h +internal/ceres/block_random_access_matrix.cc +internal/ceres/block_random_access_matrix.h +internal/ceres/block_random_access_sparse_matrix.cc +internal/ceres/block_random_access_sparse_matrix.h +internal/ceres/block_sparse_matrix.cc +internal/ceres/block_sparse_matrix.h +internal/ceres/block_structure.cc +internal/ceres/block_structure.h +internal/ceres/canonical_views_clustering.cc +internal/ceres/canonical_views_clustering.h +internal/ceres/casts.h +internal/ceres/cgnr_linear_operator.h +internal/ceres/cgnr_solver.cc +internal/ceres/cgnr_solver.h +internal/ceres/collections_port.h +internal/ceres/compressed_row_jacobian_writer.cc +internal/ceres/compressed_row_jacobian_writer.h +internal/ceres/compressed_row_sparse_matrix.cc +internal/ceres/compressed_row_sparse_matrix.h +internal/ceres/conditioned_cost_function.cc +internal/ceres/conjugate_gradients_solver.cc +internal/ceres/conjugate_gradients_solver.h +internal/ceres/corrector.cc +internal/ceres/corrector.h +internal/ceres/dense_jacobian_writer.h +internal/ceres/dense_qr_solver.cc +internal/ceres/dense_qr_solver.h +internal/ceres/dense_sparse_matrix.cc +internal/ceres/dense_sparse_matrix.h +internal/ceres/detect_structure.cc +internal/ceres/detect_structure.h +internal/ceres/evaluator.cc +internal/ceres/evaluator.h +internal/ceres/file.cc +internal/ceres/file.h +internal/ceres/generated/schur_eliminator_2_2_2.cc +internal/ceres/generated/schur_eliminator_2_2_3.cc +internal/ceres/generated/schur_eliminator_2_2_4.cc +internal/ceres/generated/schur_eliminator_2_2_d.cc +internal/ceres/generated/schur_eliminator_2_3_3.cc +internal/ceres/generated/schur_eliminator_2_3_4.cc +internal/ceres/generated/schur_eliminator_2_3_9.cc +internal/ceres/generated/schur_eliminator_2_3_d.cc +internal/ceres/generated/schur_eliminator_2_4_3.cc +internal/ceres/generated/schur_eliminator_2_4_4.cc +internal/ceres/generated/schur_eliminator_2_4_d.cc +internal/ceres/generated/schur_eliminator_4_4_2.cc +internal/ceres/generated/schur_eliminator_4_4_3.cc +internal/ceres/generated/schur_eliminator_4_4_4.cc +internal/ceres/generated/schur_eliminator_4_4_d.cc +internal/ceres/generated/schur_eliminator_d_d_d.cc +internal/ceres/gradient_checking_cost_function.cc +internal/ceres/gradient_checking_cost_function.h +internal/ceres/graph_algorithms.h +internal/ceres/graph.h +internal/ceres/implicit_schur_complement.cc +internal/ceres/implicit_schur_complement.h +internal/ceres/integral_types.h +internal/ceres/iterative_schur_complement_solver.cc +internal/ceres/iterative_schur_complement_solver.h +internal/ceres/levenberg_marquardt.cc +internal/ceres/levenberg_marquardt.h +internal/ceres/linear_least_squares_problems.cc +internal/ceres/linear_least_squares_problems.h +internal/ceres/linear_operator.cc +internal/ceres/linear_operator.h +internal/ceres/linear_solver.cc +internal/ceres/linear_solver.h +internal/ceres/local_parameterization.cc +internal/ceres/loss_function.cc +internal/ceres/map_util.h +internal/ceres/matrix_proto.h +internal/ceres/minimizer.h +internal/ceres/mutex.h +internal/ceres/normal_prior.cc +internal/ceres/parameter_block.h +internal/ceres/partitioned_matrix_view.cc +internal/ceres/partitioned_matrix_view.h +internal/ceres/problem.cc +internal/ceres/problem_impl.cc +internal/ceres/problem_impl.h +internal/ceres/program.cc +internal/ceres/program_evaluator.h +internal/ceres/program.h +internal/ceres/random.h +internal/ceres/residual_block.cc +internal/ceres/residual_block.h +internal/ceres/residual_block_utils.cc +internal/ceres/residual_block_utils.h +internal/ceres/runtime_numeric_diff_cost_function.cc +internal/ceres/runtime_numeric_diff_cost_function.h +internal/ceres/schur_complement_solver.cc +internal/ceres/schur_complement_solver.h +internal/ceres/schur_eliminator.cc +internal/ceres/schur_eliminator.h +internal/ceres/schur_eliminator_impl.h +internal/ceres/schur_ordering.cc +internal/ceres/schur_ordering.h +internal/ceres/scratch_evaluate_preparer.cc +internal/ceres/scratch_evaluate_preparer.h +internal/ceres/solver.cc +internal/ceres/solver_impl.cc +internal/ceres/solver_impl.h +internal/ceres/sparse_matrix.cc +internal/ceres/sparse_matrix.h +internal/ceres/sparse_normal_cholesky_solver.cc +internal/ceres/sparse_normal_cholesky_solver.h +internal/ceres/split.cc +internal/ceres/stl_util.h +internal/ceres/stringprintf.cc +internal/ceres/stringprintf.h +internal/ceres/suitesparse.cc +internal/ceres/suitesparse.h +internal/ceres/triplet_sparse_matrix.cc +internal/ceres/triplet_sparse_matrix.h +internal/ceres/types.cc +internal/ceres/visibility_based_preconditioner.cc +internal/ceres/visibility_based_preconditioner.h +internal/ceres/visibility.cc +internal/ceres/visibility.h diff --git a/extern/libmv/third_party/ceres/include/ceres/autodiff_cost_function.h b/extern/libmv/third_party/ceres/include/ceres/autodiff_cost_function.h new file mode 100644 index 00000000000..e86d6993864 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/autodiff_cost_function.h @@ -0,0 +1,211 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Helpers for making CostFunctions as needed by the least squares framework, +// with Jacobians computed via automatic differentiation. For more information +// on automatic differentation, see the wikipedia article at +// http://en.wikipedia.org/wiki/Automatic_differentiation +// +// To get an auto differentiated cost function, you must define a class with a +// templated operator() (a functor) that computes the cost function in terms of +// the template parameter T. The autodiff framework substitutes appropriate +// "jet" objects for T in order to compute the derivative when necessary, but +// this is hidden, and you should write the function as if T were a scalar type +// (e.g. a double-precision floating point number). +// +// The function must write the computed value in the last argument (the only +// non-const one) and return true to indicate success. +// +// For example, consider a scalar error e = k - x'y, where both x and y are +// two-dimensional column vector parameters, the prime sign indicates +// transposition, and k is a constant. The form of this error, which is the +// difference between a constant and an expression, is a common pattern in least +// squares problems. For example, the value x'y might be the model expectation +// for a series of measurements, where there is an instance of the cost function +// for each measurement k. +// +// The actual cost added to the total problem is e^2, or (k - x'k)^2; however, +// the squaring is implicitly done by the optimization framework. +// +// To write an auto-differentiable cost function for the above model, first +// define the object +// +// class MyScalarCostFunction { +// MyScalarCostFunction(double k): k_(k) {} +// +// template <typename T> +// bool operator()(const T* const x , const T* const y, T* e) const { +// e[0] = T(k_) - x[0] * y[0] + x[1] * y[1]; +// return true; +// } +// +// private: +// double k_; +// }; +// +// Note that in the declaration of operator() the input parameters x and y come +// first, and are passed as const pointers to arrays of T. If there were three +// input parameters, then the third input parameter would come after y. The +// output is always the last parameter, and is also a pointer to an array. In +// the example above, e is a scalar, so only e[0] is set. +// +// Then given this class definition, the auto differentiated cost function for +// it can be constructed as follows. +// +// CostFunction* cost_function +// = new AutoDiffCostFunction<MyScalarCostFunction, 1, 2, 2>( +// new MyScalarCostFunction(1.0)); ^ ^ ^ +// | | | +// Dimension of residual ------+ | | +// Dimension of x ----------------+ | +// Dimension of y -------------------+ +// +// In this example, there is usually an instance for each measumerent of k. +// +// In the instantiation above, the template parameters following +// "MyScalarCostFunction", "1, 2, 2", describe the functor as computing a +// 1-dimensional output from two arguments, both 2-dimensional. +// +// The autodiff cost function also supports cost functions with a +// runtime-determined number of residuals. For example: +// +// CostFunction* cost_function +// = new AutoDiffCostFunction<MyScalarCostFunction, DYNAMIC, 2, 2>( +// new CostFunctionWithDynamicNumResiduals(1.0), ^ ^ ^ +// runtime_number_of_residuals); <----+ | | | +// | | | | +// | | | | +// Actual number of residuals ------+ | | | +// Indicate dynamic number of residuals ---------+ | | +// Dimension of x -------------------------------------+ | +// Dimension of y ----------------------------------------+ +// +// The framework can currently accommodate cost functions of up to 6 independent +// variables, and there is no limit on the dimensionality of each of them. +// +// WARNING #1: Since the functor will get instantiated with different types for +// T, you must to convert from other numeric types to T before mixing +// computations with other variables of type T. In the example above, this is +// seen where instead of using k_ directly, k_ is wrapped with T(k_). +// +// WARNING #2: A common beginner's error when first using autodiff cost +// functions is to get the sizing wrong. In particular, there is a tendency to +// set the template parameters to (dimension of residual, number of parameters) +// instead of passing a dimension parameter for *every parameter*. In the +// example above, that would be <MyScalarCostFunction, 1, 2>, which is missing +// the last '2' argument. Please be careful when setting the size parameters. + +#ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ + +#include <glog/logging.h> +#include "ceres/internal/autodiff.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/sized_cost_function.h" +#include "ceres/types.h" + +namespace ceres { + +// A cost function which computes the derivative of the cost with respect to +// the parameters (a.k.a. the jacobian) using an autodifferentiation framework. +// The first template argument is the functor object, described in the header +// comment. The second argument is the dimension of the residual (or +// ceres::DYNAMIC to indicate it will be set at runtime), and subsequent +// arguments describe the size of the Nth parameter, one per parameter. +// +// The constructors take ownership of the cost functor. +// +// If the number of residuals (argument "M" below) is ceres::DYNAMIC, then the +// two-argument constructor must be used. The second constructor takes a number +// of residuals (in addition to the templated number of residuals). This allows +// for varying the number of residuals for a single autodiff cost function at +// runtime. +template <typename CostFunctor, + int M, // Number of residuals, or ceres::DYNAMIC. + int N0, // Number of parameters in block 0. + int N1 = 0, // Number of parameters in block 1. + int N2 = 0, // Number of parameters in block 2. + int N3 = 0, // Number of parameters in block 3. + int N4 = 0, // Number of parameters in block 4. + int N5 = 0> // Number of parameters in block 5. +class AutoDiffCostFunction : + public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> { + public: + // Takes ownership of functor. Uses the template-provided value for the + // number of residuals ("M"). + explicit AutoDiffCostFunction(CostFunctor* functor) + : functor_(functor) { + CHECK_NE(M, DYNAMIC) << "Can't run the fixed-size constructor if the " + << "number of residuals is set to ceres::DYNAMIC."; + } + + // Takes ownership of functor. Ignores the template-provided number of + // residuals ("M") in favor of the "num_residuals" argument provided. + // + // This allows for having autodiff cost functions which return varying + // numbers of residuals at runtime. + AutoDiffCostFunction(CostFunctor* functor, int num_residuals) + : functor_(functor) { + CHECK_EQ(M, DYNAMIC) << "Can't run the dynamic-size constructor if the " + << "number of residuals is not ceres::DYNAMIC."; + SizedCostFunction<M, N0, N1, N2, N3, N4, N5>::set_num_residuals(num_residuals); + } + + virtual ~AutoDiffCostFunction() {} + + // Implementation details follow; clients of the autodiff cost function should + // not have to examine below here. + // + // To handle varardic cost functions, some template magic is needed. It's + // mostly hidden inside autodiff.h. + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + if (!jacobians) { + return internal::VariadicEvaluate< + CostFunctor, double, N0, N1, N2, N3, N4, N5> + ::Call(*functor_, parameters, residuals); + } + return internal::AutoDiff<CostFunctor, double, + N0, N1, N2, N3, N4, N5>::Differentiate( + *functor_, + parameters, + SizedCostFunction<M, N0, N1, N2, N3, N4, N5>::num_residuals(), + residuals, + jacobians); + } + + private: + internal::scoped_ptr<CostFunctor> functor_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/ceres.h b/extern/libmv/third_party/ceres/include/ceres/ceres.h new file mode 100644 index 00000000000..22aaf8ff21a --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/ceres.h @@ -0,0 +1,48 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// This is a forwarding header containing the public symbols exported from +// Ceres. Anything in the "ceres" namespace is available for use. + +#ifndef CERES_PUBLIC_CERES_H_ +#define CERES_PUBLIC_CERES_H_ + +#include "ceres/autodiff_cost_function.h" +#include "ceres/cost_function.h" +#include "ceres/iteration_callback.h" +#include "ceres/local_parameterization.h" +#include "ceres/loss_function.h" +#include "ceres/numeric_diff_cost_function.h" +#include "ceres/problem.h" +#include "ceres/sized_cost_function.h" +#include "ceres/solver.h" +#include "ceres/types.h" + +#endif // CERES_PUBLIC_CERES_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/conditioned_cost_function.h b/extern/libmv/third_party/ceres/include/ceres/conditioned_cost_function.h new file mode 100644 index 00000000000..498d36ee55a --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/conditioned_cost_function.h @@ -0,0 +1,97 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: wjr@google.com (William Rucklidge) +// +// This file contains a cost function that can apply a transformation to +// each residual value before they are square-summed. + +#ifndef CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ +#define CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ + +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +// This class allows you to apply different conditioning to the residual +// values of a wrapped cost function. An example where this is useful is +// where you have an existing cost function that produces N values, but you +// want the total cost to be something other than just the sum of these +// squared values - maybe you want to apply a different scaling to some +// values, to change their contribution to the cost. +// +// Usage: +// +// // my_cost_function produces N residuals +// CostFunction* my_cost_function = ... +// CHECK_EQ(N, my_cost_function->num_residuals()); +// vector<CostFunction*> conditioners; +// +// // Make N 1x1 cost functions (1 parameter, 1 residual) +// CostFunction* f_1 = ... +// conditioners.push_back(f_1); +// ... +// CostFunction* f_N = ... +// conditioners.push_back(f_N); +// ConditionedCostFunction* ccf = +// new ConditionedCostFunction(my_cost_function, conditioners); +// +// Now ccf's residual i (i=0..N-1) will be passed though the i'th conditioner. +// +// ccf_residual[i] = f_i(my_cost_function_residual[i]) +// +// and the Jacobian will be affected appropriately. +class ConditionedCostFunction : public CostFunction { + public: + // Builds a cost function based on a wrapped cost function, and a + // per-residual conditioner. Takes ownership of all of the wrapped cost + // functions, or not, depending on the ownership parameter. Conditioners + // may be NULL, in which case the corresponding residual is not modified. + ConditionedCostFunction(CostFunction* wrapped_cost_function, + const vector<CostFunction*>& conditioners, + Ownership ownership); + virtual ~ConditionedCostFunction(); + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const; + + private: + internal::scoped_ptr<CostFunction> wrapped_cost_function_; + vector<CostFunction*> conditioners_; + Ownership ownership_; +}; + +} // namespace ceres + + +#endif // CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/cost_function.h b/extern/libmv/third_party/ceres/include/ceres/cost_function.h new file mode 100644 index 00000000000..84403d90636 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/cost_function.h @@ -0,0 +1,127 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.m (Keir Mierle) +// +// This is the interface through which the least squares solver accesses the +// residual and Jacobian of the least squares problem. Users are expected to +// subclass CostFunction to define their own terms in the least squares problem. +// +// It is recommended that users define templated residual functors for use as +// arguments for AutoDiffCostFunction (see autodiff_cost_function.h), instead of +// directly implementing the CostFunction interface. This often results in both +// shorter code and faster execution than hand-coded derivatives. However, +// specialized cases may demand direct implementation of the lower-level +// CostFunction interface; for example, this is true when calling legacy code +// which is not templated on numeric types. + +#ifndef CERES_PUBLIC_COST_FUNCTION_H_ +#define CERES_PUBLIC_COST_FUNCTION_H_ + +#include <vector> +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { + +// This class implements the computation of the cost (a.k.a. residual) terms as +// a function of the input (control) variables, and is the interface for users +// to describe their least squares problem to Ceres. In other words, this is the +// modelling layer between users and the Ceres optimizer. The signature of the +// function (number and sizes of input parameter blocks and number of outputs) +// is stored in parameter_block_sizes_ and num_residuals_ respectively. User +// code inheriting from this class is expected to set these two members with the +// corresponding accessors. This information will be verified by the Problem +// when added with AddResidualBlock(). +class CostFunction { + public: + CostFunction() : num_residuals_(0) {} + + virtual ~CostFunction() {} + + // Inputs: + // + // parameters is an array of pointers to arrays containing the + // various parameter blocks. parameters has the same number of + // elements as parameter_block_sizes_. Parameter blocks are in the + // same order as parameter_block_sizes_.i.e., + // + // parameters_[i] = double[parameter_block_sizes_[i]] + // + // Outputs: + // + // residuals is an array of size num_residuals_. + // + // jacobians is an array of size parameter_block_sizes_ containing + // pointers to storage for jacobian blocks corresponding to each + // parameter block. Jacobian blocks are in the same order as + // parameter_block_sizes, i.e. jacobians[i], is an + // array that contains num_residuals_* parameter_block_sizes_[i] + // elements. Each jacobian block is stored in row-major order, i.e., + // + // jacobians[i][r*parameter_block_size_[i] + c] = + // d residual[r] / d parameters[i][c] + // + // If jacobians is NULL, then no derivatives are returned; this is + // the case when computing cost only. If jacobians[i] is NULL, then + // the jacobian block corresponding to the i'th parameter block must + // not to be returned. + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const = 0; + + const vector<int16>& parameter_block_sizes() const { + return parameter_block_sizes_; + } + + int num_residuals() const { + return num_residuals_; + } + + protected: + vector<int16>* mutable_parameter_block_sizes() { + return ¶meter_block_sizes_; + } + + void set_num_residuals(int num_residuals) { + num_residuals_ = num_residuals; + } + + private: + // Cost function signature metadata: number of inputs & their sizes, + // number of outputs (residuals). + vector<int16> parameter_block_sizes_; + int num_residuals_; + DISALLOW_COPY_AND_ASSIGN(CostFunction); +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/autodiff.h b/extern/libmv/third_party/ceres/include/ceres/internal/autodiff.h new file mode 100644 index 00000000000..4f5081f8f66 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/autodiff.h @@ -0,0 +1,370 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Computation of the Jacobian matrix for vector-valued functions of multiple +// variables, using automatic differentiation based on the implementation of +// dual numbers in jet.h. Before reading the rest of this file, it is adivsable +// to read jet.h's header comment in detail. +// +// The helper wrapper AutoDiff::Differentiate() computes the jacobian of +// functors with templated operator() taking this form: +// +// struct F { +// template<typename T> +// bool operator(const T *x, const T *y, ..., T *z) { +// // Compute z[] based on x[], y[], ... +// // return true if computation succeeded, false otherwise. +// } +// }; +// +// All inputs and outputs may be vector-valued. +// +// To understand how jets are used to compute the jacobian, a +// picture may help. Consider a vector-valued function, F, returning 3 +// dimensions and taking a vector-valued parameter of 4 dimensions: +// +// y x +// [ * ] F [ * ] +// [ * ] <--- [ * ] +// [ * ] [ * ] +// [ * ] +// +// Similar to the 2-parameter example for f described in jet.h, computing the +// jacobian dy/dx is done by substutiting a suitable jet object for x and all +// intermediate steps of the computation of F. Since x is has 4 dimensions, use +// a Jet<double, 4>. +// +// Before substituting a jet object for x, the dual components are set +// appropriately for each dimension of x: +// +// y x +// [ * | * * * * ] f [ * | 1 0 0 0 ] x0 +// [ * | * * * * ] <--- [ * | 0 1 0 0 ] x1 +// [ * | * * * * ] [ * | 0 0 1 0 ] x2 +// ---+--- [ * | 0 0 0 1 ] x3 +// | ^ ^ ^ ^ +// dy/dx | | | +----- infinitesimal for x3 +// | | +------- infinitesimal for x2 +// | +--------- infinitesimal for x1 +// +----------- infinitesimal for x0 +// +// The reason to set the internal 4x4 submatrix to the identity is that we wish +// to take the derivative of y separately with respect to each dimension of x. +// Each column of the 4x4 identity is therefore for a single component of the +// independent variable x. +// +// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the +// extended y vector, indicated in the above diagram. +// +// Functors with multiple parameters +// --------------------------------- +// In practice, it is often convenient to use a function f of two or more +// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet +// framework is designed for a single-parameter vector-valued input. The wrapper +// in this file addresses this issue adding support for functions with one or +// more parameter vectors. +// +// To support multiple parameters, all the parameter vectors are concatenated +// into one and treated as a single parameter vector, except that since the +// functor expects different inputs, we need to construct the jets as if they +// were part of a single parameter vector. The extended jets are passed +// separately for each parameter. +// +// For example, consider a functor F taking two vector parameters, p[2] and +// q[3], and producing an output y[4]: +// +// struct F { +// template<typename T> +// bool operator(const T *p, const T *q, T *z) { +// // ... +// } +// }; +// +// In this case, the necessary jet type is Jet<double, 5>. Here is a +// visualization of the jet objects in this case: +// +// Dual components for p ----+ +// | +// -+- +// y [ * | 1 0 | 0 0 0 ] --- p[0] +// [ * | 0 1 | 0 0 0 ] --- p[1] +// [ * | . . | + + + ] | +// [ * | . . | + + + ] v +// [ * | . . | + + + ] <--- F(p, q) +// [ * | . . | + + + ] ^ +// ^^^ ^^^^^ | +// dy/dp dy/dq [ * | 0 0 | 1 0 0 ] --- q[0] +// [ * | 0 0 | 0 1 0 ] --- q[1] +// [ * | 0 0 | 0 0 1 ] --- q[2] +// --+-- +// | +// Dual components for q --------------+ +// +// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+" +// of y in the above diagram are the derivatives of y with respect to p and q +// respectively. This is how autodiff works for functors taking multiple vector +// valued arguments (up to 6). +// +// Jacobian NULL pointers +// ---------------------- +// In general, the functions below will accept NULL pointers for all or some of +// the Jacobian parameters, meaning that those Jacobians will not be computed. + +#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_ +#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_ + +#include <stddef.h> + +#include <glog/logging.h> +#include "ceres/jet.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" + +namespace ceres { +namespace internal { + +// Extends src by a 1st order pertubation for every dimension and puts it in +// dst. The size of src is N. Since this is also used for perturbations in +// blocked arrays, offset is used to shift which part of the jet the +// perturbation occurs. This is used to set up the extended x augmented by an +// identity matrix. The JetT type should be a Jet type, and T should be a +// numeric type (e.g. double). For example, +// +// 0 1 2 3 4 5 6 7 8 +// dst[0] [ * | . . | 1 0 0 | . . . ] +// dst[1] [ * | . . | 0 1 0 | . . . ] +// dst[2] [ * | . . | 0 0 1 | . . . ] +// +// is what would get put in dst if N was 3, offset was 3, and the jet type JetT +// was 8-dimensional. +template <typename JetT, typename T> +inline void Make1stOrderPerturbation(int offset, int N, const T *src, + JetT *dst) { + DCHECK(src); + DCHECK(dst); + for (int j = 0; j < N; ++j) { + dst[j] = JetT(src[j], offset + j); + } +} + +// Takes the 0th order part of src, assumed to be a Jet type, and puts it in +// dst. This is used to pick out the "vector" part of the extended y. +template <typename JetT, typename T> +inline void Take0thOrderPart(int M, const JetT *src, T dst) { + DCHECK(src); + for (int i = 0; i < M; ++i) { + dst[i] = src[i].a; + } +} + +// Takes N 1st order parts, starting at index N0, and puts them in the M x N +// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y. +template <typename JetT, typename T, int N0, int N> +inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { + DCHECK(src); + DCHECK(dst); + for (int i = 0; i < M; ++i) { + Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) = src[i].v.template segment<N>(N0); + } +} + +// This block of quasi-repeated code calls the user-supplied functor, which may +// take a variable number of arguments. This is accomplished by specializing the +// struct based on the size of the trailing parameters; parameters with 0 size +// are assumed missing. +// +// Supporting variadic functions is the primary source of complexity in the +// autodiff implementation. + +template<typename Functor, typename T, + int N0, int N1, int N2, int N3, int N4, int N5> +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + output); + } +}; + +template<typename Functor, typename T, + int N0, int N1, int N2, int N3, int N4> +struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + output); + } +}; + +template<typename Functor, typename T, + int N0, int N1, int N2, int N3> +struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + output); + } +}; + +template<typename Functor, typename T, + int N0, int N1, int N2> +struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + output); + } +}; + +template<typename Functor, typename T, + int N0, int N1> +struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + output); + } +}; + +template<typename Functor, typename T, int N0> +struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + output); + } +}; + +// This is in a struct because default template parameters on a function are not +// supported in C++03 (though it is available in C++0x). N0 through N5 are the +// dimension of the input arguments to the user supplied functor. +template <typename Functor, typename T, + int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5=0> +struct AutoDiff { + static bool Differentiate(const Functor& functor, + T const *const *parameters, + int num_outputs, + T *function_value, + T **jacobians) { + typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5> JetT; + + DCHECK_GT(N0, 0) + << "Cost functions must have at least one parameter block."; + DCHECK((!N1 && !N2 && !N3 && !N4 && !N5) || + ((N1 > 0) && !N2 && !N3 && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0))) + << "Zero block cannot precede a non-zero block. Block sizes are " + << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " + << N3 << ", " << N4 << ", " << N5; + + DCHECK_GT(num_outputs, 0); + + FixedArray<JetT, (256 * 7) / sizeof(JetT)> x( + N0 + N1 + N2 + N3 + N4 + N5 + num_outputs); + + // It's ugly, but it works. + const int jet0 = 0; + const int jet1 = N0; + const int jet2 = N0 + N1; + const int jet3 = N0 + N1 + N2; + const int jet4 = N0 + N1 + N2 + N3; + const int jet5 = N0 + N1 + N2 + N3 + N4; + const int jet6 = N0 + N1 + N2 + N3 + N4 + N5; + + const JetT *unpacked_parameters[6] = { + x.get() + jet0, + x.get() + jet1, + x.get() + jet2, + x.get() + jet3, + x.get() + jet4, + x.get() + jet5, + }; + JetT *output = x.get() + jet6; + +#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ + if (N ## i) { \ + internal::Make1stOrderPerturbation(jet ## i, \ + N ## i, \ + parameters[i], \ + x.get() + jet ## i); \ + } + CERES_MAKE_1ST_ORDER_PERTURBATION(0); + CERES_MAKE_1ST_ORDER_PERTURBATION(1); + CERES_MAKE_1ST_ORDER_PERTURBATION(2); + CERES_MAKE_1ST_ORDER_PERTURBATION(3); + CERES_MAKE_1ST_ORDER_PERTURBATION(4); + CERES_MAKE_1ST_ORDER_PERTURBATION(5); +#undef CERES_MAKE_1ST_ORDER_PERTURBATION + + if (!VariadicEvaluate<Functor, JetT, + N0, N1, N2, N3, N4, N5>::Call( + functor, unpacked_parameters, output)) { + return false; + } + + internal::Take0thOrderPart(num_outputs, output, function_value); + +#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \ + if (N ## i) { \ + if (jacobians[i]) { \ + internal::Take1stOrderPart<JetT, T, \ + jet ## i, \ + N ## i>(num_outputs, \ + output, \ + jacobians[i]); \ + } \ + } + CERES_TAKE_1ST_ORDER_PERTURBATION(0); + CERES_TAKE_1ST_ORDER_PERTURBATION(1); + CERES_TAKE_1ST_ORDER_PERTURBATION(2); + CERES_TAKE_1ST_ORDER_PERTURBATION(3); + CERES_TAKE_1ST_ORDER_PERTURBATION(4); + CERES_TAKE_1ST_ORDER_PERTURBATION(5); +#undef CERES_TAKE_1ST_ORDER_PERTURBATION + return true; + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/eigen.h b/extern/libmv/third_party/ceres/include/ceres/internal/eigen.h new file mode 100644 index 00000000000..be76f9eff98 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/eigen.h @@ -0,0 +1,80 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_EIGEN_H_ +#define CERES_INTERNAL_EIGEN_H_ + +#include "Eigen/Core" + +namespace ceres { + +using Eigen::Dynamic; +using Eigen::RowMajor; + +typedef Eigen::Matrix<double, Dynamic, 1> Vector; +typedef Eigen::Matrix<double, Dynamic, Dynamic, RowMajor> Matrix; +typedef Eigen::Map<Vector> VectorRef; +typedef Eigen::Map<Matrix> MatrixRef; +typedef Eigen::Map<Matrix, Eigen::Aligned> AlignedMatrixRef; +typedef Eigen::Map<const Vector> ConstVectorRef; +typedef Eigen::Map<const Matrix, Eigen::Aligned> ConstAlignedMatrixRef; +typedef Eigen::Map<const Matrix> ConstMatrixRef; + +// C++ does not support templated typdefs, thus the need for this +// struct so that we can support statically sized Matrix and Maps. +template <int num_rows = Eigen::Dynamic, int num_cols = Eigen::Dynamic> +struct EigenTypes { + typedef Eigen::Matrix <double, num_rows, num_cols, RowMajor> + Matrix; + + typedef Eigen::Map< + Eigen::Matrix<double, num_rows, num_cols, RowMajor> > + MatrixRef; + + typedef Eigen::Matrix <double, num_rows, 1> + Vector; + + typedef Eigen::Map < + Eigen::Matrix<double, num_rows, 1> > + VectorRef; + + + typedef Eigen::Map< + const Eigen::Matrix<double, num_rows, num_cols, RowMajor> > + ConstMatrixRef; + + typedef Eigen::Map < + const Eigen::Matrix<double, num_rows, 1> > + ConstVectorRef; +}; + +} // namespace ceres + +#endif // CERES_INTERNAL_EIGEN_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/fixed_array.h b/extern/libmv/third_party/ceres/include/ceres/internal/fixed_array.h new file mode 100644 index 00000000000..84617c4fa06 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/fixed_array.h @@ -0,0 +1,194 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: rennie@google.com (Jeffrey Rennie) +// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray + +#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ +#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ + +#include <cstddef> +#include <glog/logging.h> +#include "ceres/internal/manual_constructor.h" + +namespace ceres { +namespace internal { + +// A FixedArray<T> represents a non-resizable array of T where the +// length of the array does not need to be a compile time constant. +// +// FixedArray allocates small arrays inline, and large arrays on +// the heap. It is a good replacement for non-standard and deprecated +// uses of alloca() and variable length arrays (a GCC extension). +// +// FixedArray keeps performance fast for small arrays, because it +// avoids heap operations. It also helps reduce the chances of +// accidentally overflowing your stack if large input is passed to +// your function. +// +// Also, FixedArray is useful for writing portable code. Not all +// compilers support arrays of dynamic size. + +// Most users should not specify an inline_elements argument and let +// FixedArray<> automatically determine the number of elements +// to store inline based on sizeof(T). +// +// If inline_elements is specified, the FixedArray<> implementation +// will store arrays of length <= inline_elements inline. +// +// Finally note that unlike vector<T> FixedArray<T> will not zero-initialize +// simple types like int, double, bool, etc. +// +// Non-POD types will be default-initialized just like regular vectors or +// arrays. + +#if defined(_WIN64) + typedef __int64 ssize_t; +#elif defined(_WIN32) + typedef __int32 ssize_t; +#endif + +template <typename T, ssize_t inline_elements = -1> +class FixedArray { + public: + // For playing nicely with stl: + typedef T value_type; + typedef T* iterator; + typedef T const* const_iterator; + typedef T& reference; + typedef T const& const_reference; + typedef T* pointer; + typedef std::ptrdiff_t difference_type; + typedef size_t size_type; + + // REQUIRES: n >= 0 + // Creates an array object that can store "n" elements. + // + // FixedArray<T> will not zero-initialiaze POD (simple) types like int, + // double, bool, etc. + // Non-POD types will be default-initialized just like regular vectors or + // arrays. + explicit FixedArray(size_type n); + + // Releases any resources. + ~FixedArray(); + + // Returns the length of the array. + inline size_type size() const { return size_; } + + // Returns the memory size of the array in bytes. + inline size_t memsize() const { return size_ * sizeof(T); } + + // Returns a pointer to the underlying element array. + inline const T* get() const { return &array_[0].element; } + inline T* get() { return &array_[0].element; } + + // REQUIRES: 0 <= i < size() + // Returns a reference to the "i"th element. + inline T& operator[](size_type i) { + DCHECK_GE(i, 0); + DCHECK_LT(i, size_); + return array_[i].element; + } + + // REQUIRES: 0 <= i < size() + // Returns a reference to the "i"th element. + inline const T& operator[](size_type i) const { + DCHECK_GE(i, 0); + DCHECK_LT(i, size_); + return array_[i].element; + } + + inline iterator begin() { return &array_[0].element; } + inline iterator end() { return &array_[size_].element; } + + inline const_iterator begin() const { return &array_[0].element; } + inline const_iterator end() const { return &array_[size_].element; } + + private: + // Container to hold elements of type T. This is necessary to handle + // the case where T is a a (C-style) array. The size of InnerContainer + // and T must be the same, otherwise callers' assumptions about use + // of this code will be broken. + struct InnerContainer { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + T element; + }; + + // How many elements should we store inline? + // a. If not specified, use a default of 256 bytes (256 bytes + // seems small enough to not cause stack overflow or unnecessary + // stack pollution, while still allowing stack allocation for + // reasonably long character arrays. + // b. Never use 0 length arrays (not ISO C++) + static const size_type S1 = ((inline_elements < 0) + ? (256/sizeof(T)) : inline_elements); + static const size_type S2 = (S1 <= 0) ? 1 : S1; + static const size_type kInlineElements = S2; + + size_type const size_; + InnerContainer* const array_; + + // Allocate some space, not an array of elements of type T, so that we can + // skip calling the T constructors and destructors for space we never use. + ManualConstructor<InnerContainer> inline_space_[kInlineElements]; +}; + +// Implementation details follow + +template <class T, ssize_t S> +inline FixedArray<T, S>::FixedArray(typename FixedArray<T, S>::size_type n) + : size_(n), + array_((n <= kInlineElements + ? reinterpret_cast<InnerContainer*>(inline_space_) + : new InnerContainer[n])) { + DCHECK_GE(n, 0); + + // Construct only the elements actually used. + if (array_ == reinterpret_cast<InnerContainer*>(inline_space_)) { + for (int i = 0; i != size_; ++i) { + inline_space_[i].Init(); + } + } +} + +template <class T, ssize_t S> +inline FixedArray<T, S>::~FixedArray() { + if (array_ != reinterpret_cast<InnerContainer*>(inline_space_)) { + delete[] array_; + } else { + for (int i = 0; i != size_; ++i) { + inline_space_[i].Destroy(); + } + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/macros.h b/extern/libmv/third_party/ceres/include/ceres/internal/macros.h new file mode 100644 index 00000000000..0cfd773bcca --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/macros.h @@ -0,0 +1,154 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// +// Various Google-specific macros. +// +// This code is compiled directly on many platforms, including client +// platforms like Windows, Mac, and embedded systems. Before making +// any changes here, make sure that you're not breaking any platforms. + +#ifndef CERES_PUBLIC_INTERNAL_MACROS_H_ +#define CERES_PUBLIC_INTERNAL_MACROS_H_ + +#include <cstddef> // For size_t. + +// A macro to disallow the copy constructor and operator= functions +// This should be used in the private: declarations for a class +// +// For disallowing only assign or copy, write the code directly, but declare +// the intend in a comment, for example: +// void operator=(const TypeName&); // DISALLOW_ASSIGN +// Note, that most uses of DISALLOW_ASSIGN and DISALLOW_COPY are broken +// semantically, one should either use disallow both or neither. Try to +// avoid these in new code. +#define DISALLOW_COPY_AND_ASSIGN(TypeName) \ + TypeName(const TypeName&); \ + void operator=(const TypeName&) + +// A macro to disallow all the implicit constructors, namely the +// default constructor, copy constructor and operator= functions. +// +// This should be used in the private: declarations for a class +// that wants to prevent anyone from instantiating it. This is +// especially useful for classes containing only static methods. +#define DISALLOW_IMPLICIT_CONSTRUCTORS(TypeName) \ + TypeName(); \ + DISALLOW_COPY_AND_ASSIGN(TypeName) + +// The arraysize(arr) macro returns the # of elements in an array arr. +// The expression is a compile-time constant, and therefore can be +// used in defining new arrays, for example. If you use arraysize on +// a pointer by mistake, you will get a compile-time error. +// +// One caveat is that arraysize() doesn't accept any array of an +// anonymous type or a type defined inside a function. In these rare +// cases, you have to use the unsafe ARRAYSIZE() macro below. This is +// due to a limitation in C++'s template system. The limitation might +// eventually be removed, but it hasn't happened yet. + +// This template function declaration is used in defining arraysize. +// Note that the function doesn't need an implementation, as we only +// use its type. +template <typename T, size_t N> +char (&ArraySizeHelper(T (&array)[N]))[N]; + +// That gcc wants both of these prototypes seems mysterious. VC, for +// its part, can't decide which to use (another mystery). Matching of +// template overloads: the final frontier. +#ifndef _WIN32 +template <typename T, size_t N> +char (&ArraySizeHelper(const T (&array)[N]))[N]; +#endif + +#define arraysize(array) (sizeof(ArraySizeHelper(array))) + +// ARRAYSIZE performs essentially the same calculation as arraysize, +// but can be used on anonymous types or types defined inside +// functions. It's less safe than arraysize as it accepts some +// (although not all) pointers. Therefore, you should use arraysize +// whenever possible. +// +// The expression ARRAYSIZE(a) is a compile-time constant of type +// size_t. +// +// ARRAYSIZE catches a few type errors. If you see a compiler error +// +// "warning: division by zero in ..." +// +// when using ARRAYSIZE, you are (wrongfully) giving it a pointer. +// You should only use ARRAYSIZE on statically allocated arrays. +// +// The following comments are on the implementation details, and can +// be ignored by the users. +// +// ARRAYSIZE(arr) works by inspecting sizeof(arr) (the # of bytes in +// the array) and sizeof(*(arr)) (the # of bytes in one array +// element). If the former is divisible by the latter, perhaps arr is +// indeed an array, in which case the division result is the # of +// elements in the array. Otherwise, arr cannot possibly be an array, +// and we generate a compiler error to prevent the code from +// compiling. +// +// Since the size of bool is implementation-defined, we need to cast +// !(sizeof(a) & sizeof(*(a))) to size_t in order to ensure the final +// result has type size_t. +// +// This macro is not perfect as it wrongfully accepts certain +// pointers, namely where the pointer size is divisible by the pointee +// size. Since all our code has to go through a 32-bit compiler, +// where a pointer is 4 bytes, this means all pointers to a type whose +// size is 3 or greater than 4 will be (righteously) rejected. +// +// Kudos to Jorg Brown for this simple and elegant implementation. +// +// - wan 2005-11-16 +// +// Starting with Visual C++ 2005, WinNT.h includes ARRAYSIZE. However, +// the definition comes from the over-broad windows.h header that +// introduces a macro, ERROR, that conflicts with the logging framework +// that Ceres uses. Instead, rename ARRAYSIZE to CERES_ARRAYSIZE. +#define CERES_ARRAYSIZE(a) \ + ((sizeof(a) / sizeof(*(a))) / \ + static_cast<size_t>(!(sizeof(a) % sizeof(*(a))))) + +// Tell the compiler to warn about unused return values for functions declared +// with this macro. The macro should be used on function declarations +// following the argument list: +// +// Sprocket* AllocateSprocket() MUST_USE_RESULT; +// +#undef MUST_USE_RESULT +#if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) \ + && !defined(COMPILER_ICC) +#define MUST_USE_RESULT __attribute__ ((warn_unused_result)) +#else +#define MUST_USE_RESULT +#endif + +#endif // CERES_PUBLIC_INTERNAL_MACROS_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/manual_constructor.h b/extern/libmv/third_party/ceres/include/ceres/internal/manual_constructor.h new file mode 100644 index 00000000000..a1d1f444e36 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/manual_constructor.h @@ -0,0 +1,214 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: kenton@google.com (Kenton Varda) +// +// ManualConstructor statically-allocates space in which to store some +// object, but does not initialize it. You can then call the constructor +// and destructor for the object yourself as you see fit. This is useful +// for memory management optimizations, where you want to initialize and +// destroy an object multiple times but only allocate it once. +// +// (When I say ManualConstructor statically allocates space, I mean that +// the ManualConstructor object itself is forced to be the right size.) + +#ifndef CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ +#define CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ + +#include <new> + +namespace ceres { +namespace internal { + +// ------- Define ALIGNED_CHAR_ARRAY -------------------------------- + +#ifndef ALIGNED_CHAR_ARRAY + +// Because MSVC and older GCCs require that the argument to their alignment +// construct to be a literal constant integer, we use a template instantiated +// at all the possible powers of two. +template<int alignment, int size> struct AlignType { }; +template<int size> struct AlignType<0, size> { typedef char result[size]; }; +#if defined(_MSC_VER) +#define BASE_PORT_H_ALIGN_ATTRIBUTE(X) __declspec(align(X)) +#define BASE_PORT_H_ALIGN_OF(T) __alignof(T) +#elif defined(__GNUC__) +#define BASE_PORT_H_ALIGN_ATTRIBUTE(X) __attribute__((aligned(X))) +#define BASE_PORT_H_ALIGN_OF(T) __alignof__(T) +#endif + +#if defined(BASE_PORT_H_ALIGN_ATTRIBUTE) + +#define BASE_PORT_H_ALIGNTYPE_TEMPLATE(X) \ + template<int size> struct AlignType<X, size> { \ + typedef BASE_PORT_H_ALIGN_ATTRIBUTE(X) char result[size]; \ + } + +BASE_PORT_H_ALIGNTYPE_TEMPLATE(1); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(2); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(4); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(8); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(16); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(32); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(64); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(128); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(256); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(512); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(1024); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(2048); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(4096); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(8192); +// Any larger and MSVC++ will complain. + +#define ALIGNED_CHAR_ARRAY(T, Size) \ + typename AlignType<BASE_PORT_H_ALIGN_OF(T), sizeof(T) * Size>::result + +#undef BASE_PORT_H_ALIGNTYPE_TEMPLATE +#undef BASE_PORT_H_ALIGN_ATTRIBUTE + +#else // defined(BASE_PORT_H_ALIGN_ATTRIBUTE) +#define ALIGNED_CHAR_ARRAY you_must_define_ALIGNED_CHAR_ARRAY_for_your_compiler +#endif // defined(BASE_PORT_H_ALIGN_ATTRIBUTE) + +#undef BASE_PORT_H_ALIGNTYPE_TEMPLATE +#undef BASE_PORT_H_ALIGN_ATTRIBUTE + +#endif // ALIGNED_CHAR_ARRAY + +template <typename Type> +class ManualConstructor { + public: + // No constructor or destructor because one of the most useful uses of + // this class is as part of a union, and members of a union cannot have + // constructors or destructors. And, anyway, the whole point of this + // class is to bypass these. + + inline Type* get() { + return reinterpret_cast<Type*>(space_); + } + inline const Type* get() const { + return reinterpret_cast<const Type*>(space_); + } + + inline Type* operator->() { return get(); } + inline const Type* operator->() const { return get(); } + + inline Type& operator*() { return *get(); } + inline const Type& operator*() const { return *get(); } + + // You can pass up to four constructor arguments as arguments of Init(). + inline void Init() { + new(space_) Type; + } + + template <typename T1> + inline void Init(const T1& p1) { + new(space_) Type(p1); + } + + template <typename T1, typename T2> + inline void Init(const T1& p1, const T2& p2) { + new(space_) Type(p1, p2); + } + + template <typename T1, typename T2, typename T3> + inline void Init(const T1& p1, const T2& p2, const T3& p3) { + new(space_) Type(p1, p2, p3); + } + + template <typename T1, typename T2, typename T3, typename T4> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4) { + new(space_) Type(p1, p2, p3, p4); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5) { + new(space_) Type(p1, p2, p3, p4, p5); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6) { + new(space_) Type(p1, p2, p3, p4, p5, p6); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7) { + new(space_) Type(p1, p2, p3, p4, p5, p6, p7); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8) { + new(space_) Type(p1, p2, p3, p4, p5, p6, p7, p8); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8, typename T9> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9) { + new(space_) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8, typename T9, typename T10> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9, const T10& p10) { + new(space_) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8, typename T9, typename T10, + typename T11> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9, const T10& p10, const T11& p11) { + new(space_) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11); + } + + inline void Destroy() { + get()->~Type(); + } + + private: + ALIGNED_CHAR_ARRAY(Type, 1) space_; +}; + +#undef ALIGNED_CHAR_ARRAY + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/port.h b/extern/libmv/third_party/ceres/include/ceres/internal/port.h new file mode 100644 index 00000000000..9a3e5cced58 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/port.h @@ -0,0 +1,44 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_PUBLIC_INTERNAL_PORT_H_ +#define CERES_PUBLIC_INTERNAL_PORT_H_ + +namespace ceres { + +// It is unfortunate that this import of the entire standard namespace is +// necessary. The reasons are historical and won't be explained here, but +// suffice to say it is not a mistake and can't be removed without breaking +// things outside of the Ceres optimization package. +using namespace std; + +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_PORT_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/scoped_ptr.h b/extern/libmv/third_party/ceres/include/ceres/internal/scoped_ptr.h new file mode 100644 index 00000000000..44f198b339d --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/scoped_ptr.h @@ -0,0 +1,311 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: jorg@google.com (Jorg Brown) +// +// This is an implementation designed to match the anticipated future TR2 +// implementation of the scoped_ptr class, and its closely-related brethren, +// scoped_array, scoped_ptr_malloc, and make_scoped_ptr. + +#ifndef CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ +#define CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ + +#include <assert.h> +#include <stdlib.h> +#include <cstddef> + +namespace ceres { +namespace internal { + +template <class C> class scoped_ptr; +template <class C, class Free> class scoped_ptr_malloc; +template <class C> class scoped_array; + +template <class C> +scoped_ptr<C> make_scoped_ptr(C *); + +// A scoped_ptr<T> is like a T*, except that the destructor of scoped_ptr<T> +// automatically deletes the pointer it holds (if any). That is, scoped_ptr<T> +// owns the T object that it points to. Like a T*, a scoped_ptr<T> may hold +// either NULL or a pointer to a T object. Also like T*, scoped_ptr<T> is +// thread-compatible, and once you dereference it, you get the threadsafety +// guarantees of T. +// +// The size of a scoped_ptr is small: sizeof(scoped_ptr<C>) == sizeof(C*) +template <class C> +class scoped_ptr { + public: + + // The element type + typedef C element_type; + + // Constructor. Defaults to intializing with NULL. + // There is no way to create an uninitialized scoped_ptr. + // The input parameter must be allocated with new. + explicit scoped_ptr(C* p = NULL) : ptr_(p) { } + + // Destructor. If there is a C object, delete it. + // We don't need to test ptr_ == NULL because C++ does that for us. + ~scoped_ptr() { + enum { type_must_be_complete = sizeof(C) }; + delete ptr_; + } + + // Reset. Deletes the current owned object, if any. + // Then takes ownership of a new object, if given. + // this->reset(this->get()) works. + void reset(C* p = NULL) { + if (p != ptr_) { + enum { type_must_be_complete = sizeof(C) }; + delete ptr_; + ptr_ = p; + } + } + + // Accessors to get the owned object. + // operator* and operator-> will assert() if there is no current object. + C& operator*() const { + assert(ptr_ != NULL); + return *ptr_; + } + C* operator->() const { + assert(ptr_ != NULL); + return ptr_; + } + C* get() const { return ptr_; } + + // Comparison operators. + // These return whether a scoped_ptr and a raw pointer refer to + // the same object, not just to two different but equal objects. + bool operator==(const C* p) const { return ptr_ == p; } + bool operator!=(const C* p) const { return ptr_ != p; } + + // Swap two scoped pointers. + void swap(scoped_ptr& p2) { + C* tmp = ptr_; + ptr_ = p2.ptr_; + p2.ptr_ = tmp; + } + + // Release a pointer. + // The return value is the current pointer held by this object. + // If this object holds a NULL pointer, the return value is NULL. + // After this operation, this object will hold a NULL pointer, + // and will not own the object any more. + C* release() { + C* retVal = ptr_; + ptr_ = NULL; + return retVal; + } + + private: + C* ptr_; + + // google3 friend class that can access copy ctor (although if it actually + // calls a copy ctor, there will be a problem) see below + friend scoped_ptr<C> make_scoped_ptr<C>(C *p); + + // Forbid comparison of scoped_ptr types. If C2 != C, it totally doesn't + // make sense, and if C2 == C, it still doesn't make sense because you should + // never have the same object owned by two different scoped_ptrs. + template <class C2> bool operator==(scoped_ptr<C2> const& p2) const; + template <class C2> bool operator!=(scoped_ptr<C2> const& p2) const; + + // Disallow evil constructors + scoped_ptr(const scoped_ptr&); + void operator=(const scoped_ptr&); +}; + +// Free functions +template <class C> +inline void swap(scoped_ptr<C>& p1, scoped_ptr<C>& p2) { + p1.swap(p2); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_ptr<C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_ptr<const C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_ptr<C>& p2) { + return p1 != p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_ptr<const C>& p2) { + return p1 != p2.get(); +} + +template <class C> +scoped_ptr<C> make_scoped_ptr(C *p) { + // This does nothing but to return a scoped_ptr of the type that the passed + // pointer is of. (This eliminates the need to specify the name of T when + // making a scoped_ptr that is used anonymously/temporarily.) From an + // access control point of view, we construct an unnamed scoped_ptr here + // which we return and thus copy-construct. Hence, we need to have access + // to scoped_ptr::scoped_ptr(scoped_ptr const &). However, it is guaranteed + // that we never actually call the copy constructor, which is a good thing + // as we would call the temporary's object destructor (and thus delete p) + // if we actually did copy some object, here. + return scoped_ptr<C>(p); +} + +// scoped_array<C> is like scoped_ptr<C>, except that the caller must allocate +// with new [] and the destructor deletes objects with delete []. +// +// As with scoped_ptr<C>, a scoped_array<C> either points to an object +// or is NULL. A scoped_array<C> owns the object that it points to. +// scoped_array<T> is thread-compatible, and once you index into it, +// the returned objects have only the threadsafety guarantees of T. +// +// Size: sizeof(scoped_array<C>) == sizeof(C*) +template <class C> +class scoped_array { + public: + + // The element type + typedef C element_type; + + // Constructor. Defaults to intializing with NULL. + // There is no way to create an uninitialized scoped_array. + // The input parameter must be allocated with new []. + explicit scoped_array(C* p = NULL) : array_(p) { } + + // Destructor. If there is a C object, delete it. + // We don't need to test ptr_ == NULL because C++ does that for us. + ~scoped_array() { + enum { type_must_be_complete = sizeof(C) }; + delete[] array_; + } + + // Reset. Deletes the current owned object, if any. + // Then takes ownership of a new object, if given. + // this->reset(this->get()) works. + void reset(C* p = NULL) { + if (p != array_) { + enum { type_must_be_complete = sizeof(C) }; + delete[] array_; + array_ = p; + } + } + + // Get one element of the current object. + // Will assert() if there is no current object, or index i is negative. + C& operator[](std::ptrdiff_t i) const { + assert(i >= 0); + assert(array_ != NULL); + return array_[i]; + } + + // Get a pointer to the zeroth element of the current object. + // If there is no current object, return NULL. + C* get() const { + return array_; + } + + // Comparison operators. + // These return whether a scoped_array and a raw pointer refer to + // the same array, not just to two different but equal arrays. + bool operator==(const C* p) const { return array_ == p; } + bool operator!=(const C* p) const { return array_ != p; } + + // Swap two scoped arrays. + void swap(scoped_array& p2) { + C* tmp = array_; + array_ = p2.array_; + p2.array_ = tmp; + } + + // Release an array. + // The return value is the current pointer held by this object. + // If this object holds a NULL pointer, the return value is NULL. + // After this operation, this object will hold a NULL pointer, + // and will not own the object any more. + C* release() { + C* retVal = array_; + array_ = NULL; + return retVal; + } + + private: + C* array_; + + // Forbid comparison of different scoped_array types. + template <class C2> bool operator==(scoped_array<C2> const& p2) const; + template <class C2> bool operator!=(scoped_array<C2> const& p2) const; + + // Disallow evil constructors + scoped_array(const scoped_array&); + void operator=(const scoped_array&); +}; + +// Free functions +template <class C> +inline void swap(scoped_array<C>& p1, scoped_array<C>& p2) { + p1.swap(p2); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_array<C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_array<const C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_array<C>& p2) { + return p1 != p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_array<const C>& p2) { + return p1 != p2.get(); +} + +// This class wraps the c library function free() in a class that can be +// passed as a template argument to scoped_ptr_malloc below. +class ScopedPtrMallocFree { + public: + inline void operator()(void* x) const { + free(x); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/iteration_callback.h b/extern/libmv/third_party/ceres/include/ceres/iteration_callback.h new file mode 100644 index 00000000000..88da992d0c5 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/iteration_callback.h @@ -0,0 +1,159 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// When an iteration callback is specified, Ceres calls the callback after each +// optimizer step and pass it an IterationSummary object, defined below. + +#ifndef CERES_PUBLIC_ITERATION_CALLBACK_H_ +#define CERES_PUBLIC_ITERATION_CALLBACK_H_ + +#include "ceres/types.h" + +namespace ceres { + +// This struct describes the state of the optimizer after each +// iteration of the minimization. +struct IterationSummary { + // Current iteration number. + int32 iteration; + + // Whether or not the algorithm made progress in this iteration. + bool step_is_successful; + + // Value of the objective function. + double cost; + + // Change in the value of the objective function in this + // iteration. This can be positive or negative. Negative change + // means that the step was not successful. + double cost_change; + + // Infinity norm of the gradient vector. + double gradient_max_norm; + + // 2-norm of the size of the step computed by the optimization + // algorithm. + double step_norm; + + // For trust region algorithms, the ratio of the actual change in + // cost and the change in the cost of the linearized approximation. + double relative_decrease; + + // Value of the regularization parameter for Levenberg-Marquardt + // algorithm at the end of the current iteration. + double mu; + + // For the inexact step Levenberg-Marquardt algorithm, this is the + // relative accuracy with which the Newton(LM) step is solved. This + // number affects only the iterative solvers capable of solving + // linear systems inexactly. Factorization-based exact solvers + // ignore it. + double eta; + + // Number of iterations taken by the linear solver to solve for the + // Newton step. + int linear_solver_iterations; + + // TODO(sameeragarwal): Change to use a higher precision timer using + // clock_gettime. + // Time (in seconds) spent inside the linear least squares solver. + int iteration_time_sec; + + // Time (in seconds) spent inside the linear least squares solver. + int linear_solver_time_sec; +}; + +// Interface for specifying callbacks that are executed at the end of +// each iteration of the Minimizer. The solver uses the return value +// of operator() to decide whether to continue solving or to +// terminate. The user can return three values. +// +// SOLVER_ABORT indicates that the callback detected an abnormal +// situation. The solver returns without updating the parameter blocks +// (unless Solver::Options::update_state_every_iteration is set +// true). Solver returns with Solver::Summary::termination_type set to +// USER_ABORT. +// +// SOLVER_TERMINATE_SUCCESSFULLY indicates that there is no need to +// optimize anymore (some user specified termination criterion has +// been met). Solver returns with Solver::Summary::termination_type +// set to USER_SUCCESS. +// +// SOLVER_CONTINUE indicates that the solver should continue +// optimizing. +// +// For example, the following Callback is used internally by Ceres to +// log the progress of the optimization. +// +// Callback for logging the state of the minimizer to STDERR or STDOUT +// depending on the user's preferences and logging level. +// +// class LoggingCallback : public IterationCallback { +// public: +// explicit LoggingCallback(bool log_to_stdout) +// : log_to_stdout_(log_to_stdout) {} +// +// ~LoggingCallback() {} +// +// CallbackReturnType operator()(const IterationSummary& summary) { +// const char* kReportRowFormat = +// "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " +// "rho:% 3.2e mu:% 3.2e eta:% 3.2e li:% 3d"; +// string output = StringPrintf(kReportRowFormat, +// summary.iteration, +// summary.cost, +// summary.cost_change, +// summary.gradient_max_norm, +// summary.step_norm, +// summary.relative_decrease, +// summary.mu, +// summary.eta, +// summary.linear_solver_iterations); +// if (log_to_stdout_) { +// cout << output << endl; +// } else { +// VLOG(1) << output; +// } +// return SOLVER_CONTINUE; +// } +// +// private: +// const bool log_to_stdout_; +// }; +// +class IterationCallback { + public: + virtual ~IterationCallback() {} + virtual CallbackReturnType operator()(const IterationSummary& summary) = 0; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_ITERATION_CALLBACK_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/jet.h b/extern/libmv/third_party/ceres/include/ceres/jet.h new file mode 100644 index 00000000000..264861735ed --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/jet.h @@ -0,0 +1,755 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A simple implementation of N-dimensional dual numbers, for automatically +// computing exact derivatives of functions. +// +// While a complete treatment of the mechanics of automatic differentation is +// beyond the scope of this header (see +// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the +// basic idea is to extend normal arithmetic with an extra element, "e," often +// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual +// numbers are extensions of the real numbers analogous to complex numbers: +// whereas complex numbers augment the reals by introducing an imaginary unit i +// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such +// that e^2 = 0. Dual numbers have two components: the "real" component and the +// "infinitesimal" component, generally written as x + y*e. Surprisingly, this +// leads to a convenient method for computing exact derivatives without needing +// to manipulate complicated symbolic expressions. +// +// For example, consider the function +// +// f(x) = x^2 , +// +// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20. +// Next, augument 10 with an infinitesimal to get: +// +// f(10 + e) = (10 + e)^2 +// = 100 + 2 * 10 * e + e^2 +// = 100 + 20 * e -+- +// -- | +// | +--- This is zero, since e^2 = 0 +// | +// +----------------- This is df/dx! +// +// Note that the derivative of f with respect to x is simply the infinitesimal +// component of the value of f(x + e). So, in order to take the derivative of +// any function, it is only necessary to replace the numeric "object" used in +// the function with one extended with infinitesimals. The class Jet, defined in +// this header, is one such example of this, where substitution is done with +// templates. +// +// To handle derivatives of functions taking multiple arguments, different +// infinitesimals are used, one for each variable to take the derivative of. For +// example, consider a scalar function of two scalar parameters x and y: +// +// f(x, y) = x^2 + x * y +// +// Following the technique above, to compute the derivatives df/dx and df/dy for +// f(1, 3) involves doing two evaluations of f, the first time replacing x with +// x + e, the second time replacing y with y + e. +// +// For df/dx: +// +// f(1 + e, y) = (1 + e)^2 + (1 + e) * 3 +// = 1 + 2 * e + 3 + 3 * e +// = 4 + 5 * e +// +// --> df/dx = 5 +// +// For df/dy: +// +// f(1, 3 + e) = 1^2 + 1 * (3 + e) +// = 1 + 3 + e +// = 4 + e +// +// --> df/dy = 1 +// +// To take the gradient of f with the implementation of dual numbers ("jets") in +// this file, it is necessary to create a single jet type which has components +// for the derivative in x and y, and passing them to a templated version of f: +// +// template<typename T> +// T f(const T &x, const T &y) { +// return x * x + x * y; +// } +// +// // The "2" means there should be 2 dual number components. +// Jet<double, 2> x(0); // Pick the 0th dual number for x. +// Jet<double, 2> y(1); // Pick the 1st dual number for y. +// Jet<double, 2> z = f(x, y); +// +// LG << "df/dx = " << z.a[0] +// << "df/dy = " << z.a[1]; +// +// Most users should not use Jet objects directly; a wrapper around Jet objects, +// which makes computing the derivative, gradient, or jacobian of templated +// functors simple, is in autodiff.h. Even autodiff.h should not be used +// directly; instead autodiff_cost_function.h is typically the file of interest. +// +// For the more mathematically inclined, this file implements first-order +// "jets". A 1st order jet is an element of the ring +// +// T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2 +// +// which essentially means that each jet consists of a "scalar" value 'a' from T +// and a 1st order perturbation vector 'v' of length N: +// +// x = a + \sum_i v[i] t_i +// +// A shorthand is to write an element as x = a + u, where u is the pertubation. +// Then, the main point about the arithmetic of jets is that the product of +// perturbations is zero: +// +// (a + u) * (b + v) = ab + av + bu + uv +// = ab + (av + bu) + 0 +// +// which is what operator* implements below. Addition is simpler: +// +// (a + u) + (b + v) = (a + b) + (u + v). +// +// The only remaining question is how to evaluate the function of a jet, for +// which we use the chain rule: +// +// f(a + u) = f(a) + f'(a) u +// +// where f'(a) is the (scalar) derivative of f at a. +// +// By pushing these things through sufficiently and suitably templated +// functions, we can do automatic differentiation. Just be sure to turn on +// function inlining and common-subexpression elimination, or it will be very +// slow! +// +// WARNING: Most Ceres users should not directly include this file or know the +// details of how jets work. Instead the suggested method for automatic +// derivatives is to use autodiff_cost_function.h, which is a wrapper around +// both jets.h and autodiff.h to make taking derivatives of cost functions for +// use in Ceres easier. + +#ifndef CERES_PUBLIC_JET_H_ +#define CERES_PUBLIC_JET_H_ + +#include <cmath> +#include <iosfwd> +#include <iostream> // NOLINT +#include <string> + +#include "Eigen/Core" + +// Visual Studio 2010 or older version +#if defined(_MSC_VER) && _MSC_VER <= 1600 +namespace std { +inline bool isfinite(double x) { return _finite(x); } +inline bool isinf (double x) { return !_finite(x) && !_isnan(x); } +inline bool isnan (double x) { return _isnan(x); } +inline bool isnormal(double x) { return _finite(x) && x != 0.0; } +} // namespace std +#endif + +namespace ceres { + +template <typename T, int N> +struct Jet { + enum { DIMENSION = N }; + + // Default-construct "a" because otherwise this can lead to false errors about + // uninitialized uses when other classes relying on default constructed T + // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that + // the C++ standard mandates that e.g. default constructed doubles are + // initialized to 0.0; see sections 8.5 of the C++03 standard. + Jet() : a() {} + + // Constructor from scalar: a + 0. + explicit Jet(const T& value) { + a = value; + v.setZero(); + } + + // Constructor from scalar plus variable: a + t_i. + Jet(const T& value, int k) { + a = value; + v.setZero(); + v[k] = T(1.0); + } + + /* + + // Construct from an array where the first element is the scalar. + // This is templated to support converting from other data types. + template<typename D> + Jet(const D* scalar_and_derivatives) { + a = T(scalar_and_derivatives[0]); + v = Eigen::Map<const Eigen::Matrix<D, N, 1> >( + scalar_and_derivatives + 1, N).cast<T>(); + } + */ + + // Compound operators + Jet<T, N>& operator+=(const Jet<T, N> &y) { + *this = *this + y; + return *this; + } + + Jet<T, N>& operator-=(const Jet<T, N> &y) { + *this = *this - y; + return *this; + } + + Jet<T, N>& operator*=(const Jet<T, N> &y) { + *this = *this * y; + return *this; + } + + Jet<T, N>& operator/=(const Jet<T, N> &y) { + *this = *this / y; + return *this; + } + + T a; // The scalar part. + Eigen::Matrix<T, N, 1> v; // The infinitesimal part. +}; + +// Unary + +template<typename T, int N> inline +Jet<T, N> const& operator+(const Jet<T, N>& f) { + return f; +} + +// TODO(keir): Try adding __attribute__((always_inline)) to these functions to +// see if it causes a performance increase. + +// Unary - +template<typename T, int N> inline +Jet<T, N> operator-(const Jet<T, N>&f) { + Jet<T, N> g; + g.a = -f.a; + g.v = -f.v; + return g; +} + +// Binary + +template<typename T, int N> inline +Jet<T, N> operator+(const Jet<T, N>& f, + const Jet<T, N>& g) { + Jet<T, N> h; + h.a = f.a + g.a; + h.v = f.v + g.v; + return h; +} + +// Binary + with a scalar: x + s +template<typename T, int N> inline +Jet<T, N> operator+(const Jet<T, N>& f, T s) { + Jet<T, N> h; + h.a = f.a + s; + h.v = f.v; + return h; +} + +// Binary + with a scalar: s + x +template<typename T, int N> inline +Jet<T, N> operator+(T s, const Jet<T, N>& f) { + Jet<T, N> h; + h.a = f.a + s; + h.v = f.v; + return h; +} + +// Binary - +template<typename T, int N> inline +Jet<T, N> operator-(const Jet<T, N>& f, + const Jet<T, N>& g) { + Jet<T, N> h; + h.a = f.a - g.a; + h.v = f.v - g.v; + return h; +} + +// Binary - with a scalar: x - s +template<typename T, int N> inline +Jet<T, N> operator-(const Jet<T, N>& f, T s) { + Jet<T, N> h; + h.a = f.a - s; + h.v = f.v; + return h; +} + +// Binary - with a scalar: s - x +template<typename T, int N> inline +Jet<T, N> operator-(T s, const Jet<T, N>& f) { + Jet<T, N> h; + h.a = s - f.a; + h.v = -f.v; + return h; +} + +// Binary * +template<typename T, int N> inline +Jet<T, N> operator*(const Jet<T, N>& f, + const Jet<T, N>& g) { + Jet<T, N> h; + h.a = f.a * g.a; + h.v = f.a * g.v + f.v * g.a; + return h; +} + +// Binary * with a scalar: x * s +template<typename T, int N> inline +Jet<T, N> operator*(const Jet<T, N>& f, T s) { + Jet<T, N> h; + h.a = f.a * s; + h.v = f.v * s; + return h; +} + +// Binary * with a scalar: s * x +template<typename T, int N> inline +Jet<T, N> operator*(T s, const Jet<T, N>& f) { + Jet<T, N> h; + h.a = f.a * s; + h.v = f.v * s; + return h; +} + +// Binary / +template<typename T, int N> inline +Jet<T, N> operator/(const Jet<T, N>& f, + const Jet<T, N>& g) { + Jet<T, N> h; + // This uses: + // + // a + u (a + u)(b - v) (a + u)(b - v) + // ----- = -------------- = -------------- + // b + v (b + v)(b - v) b^2 + // + // which holds because v*v = 0. + h.a = f.a / g.a; + h.v = (f.v - f.a / g.a * g.v) / g.a; + return h; +} + +// Binary / with a scalar: s / x +template<typename T, int N> inline +Jet<T, N> operator/(T s, const Jet<T, N>& g) { + Jet<T, N> h; + h.a = s / g.a; + h.v = - s * g.v / (g.a * g.a); + return h; +} + +// Binary / with a scalar: x / s +template<typename T, int N> inline +Jet<T, N> operator/(const Jet<T, N>& f, T s) { + Jet<T, N> h; + h.a = f.a / s; + h.v = f.v / s; + return h; +} + +// Binary comparison operators for both scalars and jets. +#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ +template<typename T, int N> inline \ +bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \ + return f.a op g.a; \ +} \ +template<typename T, int N> inline \ +bool operator op(const T& s, const Jet<T, N>& g) { \ + return s op g.a; \ +} \ +template<typename T, int N> inline \ +bool operator op(const Jet<T, N>& f, const T& s) { \ + return f.a op s; \ +} +CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT +#undef CERES_DEFINE_JET_COMPARISON_OPERATOR + +// Pull some functions from namespace std. +// +// This is necessary because we want to use the same name (e.g. 'sqrt') for +// double-valued and Jet-valued functions, but we are not allowed to put +// Jet-valued functions inside namespace std. +// +// Missing: cosh, sinh, tanh, tan +// TODO(keir): Switch to "using". +inline double abs (double x) { return std::abs(x); } +inline double log (double x) { return std::log(x); } +inline double exp (double x) { return std::exp(x); } +inline double sqrt (double x) { return std::sqrt(x); } +inline double cos (double x) { return std::cos(x); } +inline double acos (double x) { return std::acos(x); } +inline double sin (double x) { return std::sin(x); } +inline double asin (double x) { return std::asin(x); } +inline bool isfinite(double x) { return std::isfinite(x); } +inline bool isinf (double x) { return std::isinf(x); } +inline bool isnan (double x) { return std::isnan(x); } +inline bool isnormal(double x) { return std::isnormal(x); } +inline double pow (double x, double y) { return std::pow(x, y); } +inline double atan2(double y, double x) { return std::atan2(y, x); } + +// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. + +// abs(x + h) ~= x + h or -(x + h) +template <typename T, int N> inline +Jet<T, N> abs(const Jet<T, N>& f) { + return f.a < T(0.0) ? -f : f; +} + +// log(a + h) ~= log(a) + h / a +template <typename T, int N> inline +Jet<T, N> log(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = log(f.a); + g.v = f.v / f.a; + return g; +} + +// exp(a + h) ~= exp(a) + exp(a) h +template <typename T, int N> inline +Jet<T, N> exp(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = exp(f.a); + g.v = g.a * f.v; + return g; +} + +// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) +template <typename T, int N> inline +Jet<T, N> sqrt(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = sqrt(f.a); + g.v = f.v / (T(2.0) * g.a); + return g; +} + +// cos(a + h) ~= cos(a) - sin(a) h +template <typename T, int N> inline +Jet<T, N> cos(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = cos(f.a); + T sin_a = sin(f.a); + g.v = - sin_a * f.v; + return g; +} + +// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h +template <typename T, int N> inline +Jet<T, N> acos(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = acos(f.a); + g.v = - T(1.0) / sqrt(T(1.0) - f.a * f.a) * f.v; + return g; +} + +// sin(a + h) ~= sin(a) + cos(a) h +template <typename T, int N> inline +Jet<T, N> sin(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = sin(f.a); + T cos_a = cos(f.a); + g.v = cos_a * f.v; + return g; +} + +// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h +template <typename T, int N> inline +Jet<T, N> asin(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = asin(f.a); + g.v = T(1.0) / sqrt(T(1.0) - f.a * f.a) * f.v; + return g; +} + +// Jet Classification. It is not clear what the appropriate semantics are for +// these classifications. This picks that isfinite and isnormal are "all" +// operations, i.e. all elements of the jet must be finite for the jet itself to +// be finite (or normal). For isnan and isinf, the answer is less clear. This +// takes a "any" approach for isnan and isinf such that if any part of a jet is +// nan or inf, then the entire jet is nan or inf. This leads to strange +// situations like a jet can be both isinf and isnan, but in practice the "any" +// semantics are the most useful for e.g. checking that derivatives are sane. + +// The jet is finite if all parts of the jet are finite. +template <typename T, int N> inline +bool isfinite(const Jet<T, N>& f) { + if (!isfinite(f.a)) { + return false; + } + for (int i = 0; i < N; ++i) { + if (!isfinite(f.v[i])) { + return false; + } + } + return true; +} + +// The jet is infinite if any part of the jet is infinite. +template <typename T, int N> inline +bool isinf(const Jet<T, N>& f) { + if (isinf(f.a)) { + return true; + } + for (int i = 0; i < N; i++) { + if (isinf(f.v[i])) { + return true; + } + } + return false; +} + +// The jet is NaN if any part of the jet is NaN. +template <typename T, int N> inline +bool isnan(const Jet<T, N>& f) { + if (isnan(f.a)) { + return true; + } + for (int i = 0; i < N; ++i) { + if (isnan(f.v[i])) { + return true; + } + } + return false; +} + +// The jet is normal if all parts of the jet are normal. +template <typename T, int N> inline +bool isnormal(const Jet<T, N>& f) { + if (!isnormal(f.a)) { + return false; + } + for (int i = 0; i < N; ++i) { + if (!isnormal(f.v[i])) { + return false; + } + } + return true; +} + +// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) +// +// In words: the rate of change of theta is 1/r times the rate of +// change of (x, y) in the positive angular direction. +template <typename T, int N> inline +Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { + // Note order of arguments: + // + // f = a + da + // g = b + db + + Jet<T, N> out; + + out.a = atan2(g.a, f.a); + + T const temp = T(1.0) / (f.a * f.a + g.a * g.a); + out.v = temp * (- g.a * f.v + f.a * g.v); + return out; +} + + +// pow -- base is a differentiatble function, exponent is a constant. +// (a+da)^p ~= a^p + p*a^(p-1) da +template <typename T, int N> inline +Jet<T, N> pow(const Jet<T, N>& f, double g) { + Jet<T, N> out; + out.a = pow(f.a, g); + T const temp = g * pow(f.a, g - T(1.0)); + out.v = temp * f.v; + return out; +} + +// pow -- base is a constant, exponent is a differentiable function. +// (a)^(p+dp) ~= a^p + a^p log(a) dp +template <typename T, int N> inline +Jet<T, N> pow(double f, const Jet<T, N>& g) { + Jet<T, N> out; + out.a = pow(f, g.a); + T const temp = log(f) * out.a; + out.v = temp * g.v; + return out; +} + + +// pow -- both base and exponent are differentiable functions. +// (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db +template <typename T, int N> inline +Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { + Jet<T, N> out; + + T const temp1 = pow(f.a, g.a); + T const temp2 = g.a * pow(f.a, g.a - T(1.0)); + T const temp3 = temp1 * log(f.a); + + out.a = temp1; + out.v = temp2 * f.v + temp3 * g.v; + return out; +} + +// Define the helper functions Eigen needs to embed Jet types. +// +// NOTE(keir): machine_epsilon() and precision() are missing, because they don't +// work with nested template types (e.g. where the scalar is itself templated). +// Among other things, this means that decompositions of Jet's does not work, +// for example +// +// Matrix<Jet<T, N> ... > A, x, b; +// ... +// A.solve(b, &x) +// +// does not work and will fail with a strange compiler error. +// +// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we +// switch to 3.0, also add the rest of the specialization functionality. +template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x; } // NOLINT +template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x; } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_imag(const Jet<T, N>& ) { return Jet<T, N>(0.0); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_abs (const Jet<T, N>& x) { return fabs(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_abs2(const Jet<T, N>& x) { return x * x; } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_sqrt(const Jet<T, N>& x) { return sqrt(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) { return exp(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_pow (const Jet<T, N>& x, Jet<T, N> y) { return pow(x, y); } // NOLINT + +// Note: This has to be in the ceres namespace for argument dependent lookup to +// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with +// strange compile errors. +template <typename T, int N> +inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) { + return s << "[" << z.a << " ; " << z.v.transpose() << "]"; +} + +// A jet traits class to make it easier to work with mixed auto / numeric diff. +template<typename T> +struct JetOps { + static bool IsScalar() { + return true; + } + static T GetScalar(const T& t) { + return t; + } + static void SetScalar(const T& scalar, T* t) { + *t = scalar; + } + static void ScaleDerivative(double scale_by, T *value) { + // For double, there is no derivative to scale. + } +}; + +template<typename T, int N> +struct JetOps<Jet<T, N> > { + static bool IsScalar() { + return false; + } + static T GetScalar(const Jet<T, N>& t) { + return t.a; + } + static void SetScalar(const T& scalar, Jet<T, N>* t) { + t->a = scalar; + } + static void ScaleDerivative(double scale_by, Jet<T, N> *value) { + value->v *= scale_by; + } +}; + +template<typename FunctionType, int kNumArgs, typename ArgumentType> +struct Chain { + static ArgumentType Rule(const FunctionType &f, + const FunctionType dfdx[kNumArgs], + const ArgumentType x[kNumArgs]) { + // In the default case of scalars, there's nothing to do since there are no + // derivatives to propagate. + return f; + } +}; + +// XXX Add documentation here! +template<typename FunctionType, int kNumArgs, typename T, int N> +struct Chain<FunctionType, kNumArgs, Jet<T, N> > { + static Jet<T, N> Rule(const FunctionType &f, + const FunctionType dfdx[kNumArgs], + const Jet<T, N> x[kNumArgs]) { + // x is itself a function of another variable ("z"); what this function + // needs to return is "f", but with the derivative with respect to z + // attached to the jet. So combine the derivative part of x's jets to form + // a Jacobian matrix between x and z (i.e. dx/dz). + Eigen::Matrix<T, kNumArgs, N> dxdz; + for (int i = 0; i < kNumArgs; ++i) { + dxdz.row(i) = x[i].v.transpose(); + } + + // Map the input gradient dfdx into an Eigen row vector. + Eigen::Map<const Eigen::Matrix<FunctionType, 1, kNumArgs> > + vector_dfdx(dfdx, 1, kNumArgs); + + // Now apply the chain rule to obtain df/dz. Combine the derivative with + // the scalar part to obtain f with full derivative information. + Jet<T, N> jet_f; + jet_f.a = f; + jet_f.v = vector_dfdx.template cast<T>() * dxdz; // Also known as dfdz. + return jet_f; + } +}; + +} // namespace ceres + +namespace Eigen { + +// Creating a specialization of NumTraits enables placing Jet objects inside +// Eigen arrays, getting all the goodness of Eigen combined with autodiff. +template<typename T, int N> +struct NumTraits<ceres::Jet<T, N> > { + typedef ceres::Jet<T, N> Real; + typedef ceres::Jet<T, N> NonInteger; + typedef ceres::Jet<T, N> Nested; + + static typename ceres::Jet<T, N> dummy_precision() { + return ceres::Jet<T, N>(1e-12); + } + + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned, + ReadCost = 1, + AddCost = 1, + // For Jet types, multiplication is more expensive than addition. + MulCost = 3, + HasFloatingPoint = 1 + }; +}; + +} // namespace Eigen + +#endif // CERES_PUBLIC_JET_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/local_parameterization.h b/extern/libmv/third_party/ceres/include/ceres/local_parameterization.h new file mode 100644 index 00000000000..c0f7dc77a57 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/local_parameterization.h @@ -0,0 +1,189 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ +#define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ + +#include <vector> +#include "ceres/internal/port.h" + +namespace ceres { + +// Purpose: Sometimes parameter blocks x can overparameterize a problem +// +// min f(x) +// x +// +// In that case it is desirable to choose a parameterization for the +// block itself to remove the null directions of the cost. More +// generally, if x lies on a manifold of a smaller dimension than the +// ambient space that it is embedded in, then it is numerically and +// computationally more effective to optimize it using a +// parameterization that lives in the tangent space of that manifold +// at each point. +// +// For example, a sphere in three dimensions is a 2 dimensional +// manifold, embedded in a three dimensional space. At each point on +// the sphere, the plane tangent to it defines a two dimensional +// tangent space. For a cost function defined on this sphere, given a +// point x, moving in the direction normal to the sphere at that point +// is not useful. Thus a better way to do a local optimization is to +// optimize over two dimensional vector delta in the tangent space at +// that point and then "move" to the point x + delta, where the move +// operation involves projecting back onto the sphere. Doing so +// removes a redundent dimension from the optimization, making it +// numerically more robust and efficient. +// +// More generally we can define a function +// +// x_plus_delta = Plus(x, delta), +// +// where x_plus_delta has the same size as x, and delta is of size +// less than or equal to x. The function Plus, generalizes the +// definition of vector addition. Thus it satisfies the identify +// +// Plus(x, 0) = x, for all x. +// +// A trivial version of Plus is when delta is of the same size as x +// and +// +// Plus(x, delta) = x + delta +// +// A more interesting case if x is two dimensional vector, and the +// user wishes to hold the first coordinate constant. Then, delta is a +// scalar and Plus is defined as +// +// Plus(x, delta) = x + [0] * delta +// [1] +// +// An example that occurs commonly in Structure from Motion problems +// is when camera rotations are parameterized using Quaternion. There, +// it is useful only make updates orthogonal to that 4-vector defining +// the quaternion. One way to do this is to let delta be a 3 +// dimensional vector and define Plus to be +// +// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x +// +// The multiplication between the two 4-vectors on the RHS is the +// standard quaternion product. +// +// Given g and a point x, optimizing f can now be restated as +// +// min f(Plus(x, delta)) +// delta +// +// Given a solution delta to this problem, the optimal value is then +// given by +// +// x* = Plus(x, delta) +// +// The class LocalParameterization defines the function Plus and its +// Jacobian which is needed to compute the Jacobian of f w.r.t delta. +class LocalParameterization { + public: + virtual ~LocalParameterization() {} + + // Generalization of the addition operation, + // + // x_plus_delta = Plus(x, delta) + // + // with the condition that Plus(x, 0) = x. + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const = 0; + + // The jacobian of Plus(x, delta) w.r.t delta at delta = 0. + virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; + + // Size of x. + virtual int GlobalSize() const = 0; + + // Size of delta. + virtual int LocalSize() const = 0; +}; + +// Some basic parameterizations + +// Identity Parameterization: Plus(x, delta) = x + delta +class IdentityParameterization : public LocalParameterization { + public: + explicit IdentityParameterization(int size); + virtual ~IdentityParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual int GlobalSize() const { return size_; } + virtual int LocalSize() const { return size_; } + + private: + const int size_; +}; + +// Hold a subset of the parameters inside a parameter block constant. +class SubsetParameterization : public LocalParameterization { + public: + explicit SubsetParameterization(int size, + const vector<int>& constant_parameters); + virtual ~SubsetParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual int GlobalSize() const { return constancy_mask_.size(); } + virtual int LocalSize() const { return local_size_; } + + private: + const int local_size_; + vector<int> constancy_mask_; +}; + +// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x +// with * being the quaternion multiplication operator. Here we assume +// that the first element of the quaternion vector is the real (cos +// theta) part. +class QuaternionParameterization : public LocalParameterization { + public: + virtual ~QuaternionParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual int GlobalSize() const { return 4; } + virtual int LocalSize() const { return 3; } +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/loss_function.h b/extern/libmv/third_party/ceres/include/ceres/loss_function.h new file mode 100644 index 00000000000..81add02cdee --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/loss_function.h @@ -0,0 +1,322 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// The LossFunction interface is the way users describe how residuals +// are converted to cost terms for the overall problem cost function. +// For the exact manner in which loss functions are converted to the +// overall cost for a problem, see problem.h. +// +// For least squares problem where there are no outliers and standard +// squared loss is expected, it is not necessary to create a loss +// function; instead passing a NULL to the problem when adding +// residuals implies a standard squared loss. +// +// For least squares problems where the minimization may encounter +// input terms that contain outliers, that is, completely bogus +// measurements, it is important to use a loss function that reduces +// their associated penalty. +// +// Consider a structure from motion problem. The unknowns are 3D +// points and camera parameters, and the measurements are image +// coordinates describing the expected reprojected position for a +// point in a camera. For example, we want to model the geometry of a +// street scene with fire hydrants and cars, observed by a moving +// camera with unknown parameters, and the only 3D points we care +// about are the pointy tippy-tops of the fire hydrants. Our magic +// image processing algorithm, which is responsible for producing the +// measurements that are input to Ceres, has found and matched all +// such tippy-tops in all image frames, except that in one of the +// frame it mistook a car's headlight for a hydrant. If we didn't do +// anything special (i.e. if we used a basic quadratic loss), the +// residual for the erroneous measurement will result in extreme error +// due to the quadratic nature of squared loss. This results in the +// entire solution getting pulled away from the optimimum to reduce +// the large error that would otherwise be attributed to the wrong +// measurement. +// +// Using a robust loss function, the cost for large residuals is +// reduced. In the example above, this leads to outlier terms getting +// downweighted so they do not overly influence the final solution. +// +// What cost function is best? +// +// In general, there isn't a principled way to select a robust loss +// function. The authors suggest starting with a non-robust cost, then +// only experimenting with robust loss functions if standard squared +// loss doesn't work. + +#ifndef CERES_PUBLIC_LOSS_FUNCTION_H_ +#define CERES_PUBLIC_LOSS_FUNCTION_H_ + +#include <glog/logging.h> +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +class LossFunction { + public: + virtual ~LossFunction() {} + + // For a residual vector with squared 2-norm 'sq_norm', this method + // is required to fill in the value and derivatives of the loss + // function (rho in this example): + // + // out[0] = rho(sq_norm), + // out[1] = rho'(sq_norm), + // out[2] = rho''(sq_norm), + // + // Here the convention is that the contribution of a term to the + // cost function is given by 1/2 rho(s), where + // + // s = ||residuals||^2. + // + // Calling the method with a negative value of 's' is an error and + // the implementations are not required to handle that case. + // + // Most sane choices of rho() satisfy: + // + // rho(0) = 0, + // rho'(0) = 1, + // rho'(s) < 1 in outlier region, + // rho''(s) < 0 in outlier region, + // + // so that they mimic the least squares cost for small residuals. + virtual void Evaluate(double sq_norm, double out[3]) const = 0; +}; + +// Some common implementations follow below. +// +// Note: in the region of interest (i.e. s < 3) we have: +// TrivialLoss >= HuberLoss >= SoftLOneLoss >= CauchyLoss + + +// This corresponds to no robustification. +// +// rho(s) = s +// +// At s = 0: rho = [0, 1, 0]. +// +// It is not normally necessary to use this, as passing NULL for the +// loss function when building the problem accomplishes the same +// thing. +class TrivialLoss : public LossFunction { + public: + virtual void Evaluate(double, double*) const; +}; + +// Scaling +// ------- +// Given one robustifier +// s -> rho(s) +// one can change the length scale at which robustification takes +// place, by adding a scale factor 'a' as follows: +// +// s -> a^2 rho(s / a^2). +// +// The first and second derivatives are: +// +// s -> rho'(s / a^2), +// s -> (1 / a^2) rho''(s / a^2), +// +// but the behaviour near s = 0 is the same as the original function, +// i.e. +// +// rho(s) = s + higher order terms, +// a^2 rho(s / a^2) = s + higher order terms. +// +// The scalar 'a' should be positive. +// +// The reason for the appearance of squaring is that 'a' is in the +// units of the residual vector norm whereas 's' is a squared +// norm. For applications it is more convenient to specify 'a' than +// its square. The commonly used robustifiers below are described in +// un-scaled format (a = 1) but their implementations work for any +// non-zero value of 'a'. + +// Huber. +// +// rho(s) = s for s <= 1, +// rho(s) = 2 sqrt(s) - 1 for s >= 1. +// +// At s = 0: rho = [0, 1, 0]. +// +// The scaling parameter 'a' corresponds to 'delta' on this page: +// http://en.wikipedia.org/wiki/Huber_Loss_Function +class HuberLoss : public LossFunction { + public: + explicit HuberLoss(double a) : a_(a), b_(a * a) { } + virtual void Evaluate(double, double*) const; + private: + const double a_; + // b = a^2. + const double b_; +}; + +// Soft L1, similar to Huber but smooth. +// +// rho(s) = 2 (sqrt(1 + s) - 1). +// +// At s = 0: rho = [0, 1, -1/2]. +class SoftLOneLoss : public LossFunction { + public: + explicit SoftLOneLoss(double a) : b_(a * a), c_(1 / b_) { } + virtual void Evaluate(double, double*) const; + private: + // b = a^2. + const double b_; + // c = 1 / a^2. + const double c_; +}; + +// Inspired by the Cauchy distribution +// +// rho(s) = log(1 + s). +// +// At s = 0: rho = [0, 1, -1]. +class CauchyLoss : public LossFunction { + public: + explicit CauchyLoss(double a) : b_(a * a), c_(1 / b_) { } + virtual void Evaluate(double, double*) const; + private: + // b = a^2. + const double b_; + // c = 1 / a^2. + const double c_; +}; + +// The discussion above has to do with length scaling: it affects the space +// in which s is measured. Sometimes you want to simply scale the output +// value of the robustifier. For example, you might want to weight +// different error terms differently (e.g., weight pixel reprojection +// errors differently from terrain errors). +// +// If rho is the wrapped robustifier, then this simply outputs +// s -> a * rho(s) +// +// The first and second derivatives are, not surprisingly +// s -> a * rho'(s) +// s -> a * rho''(s) +// +// Since we treat the a NULL Loss function as the Identity loss +// function, rho = NULL is a valid input and will result in the input +// being scaled by a. This provides a simple way of implementing a +// scaled ResidualBlock. +class ScaledLoss : public LossFunction { + public: + // Constructs a ScaledLoss wrapping another loss function. Takes + // ownership of the wrapped loss function or not depending on the + // ownership parameter. + ScaledLoss(const LossFunction* rho, double a, Ownership ownership) : + rho_(rho), a_(a), ownership_(ownership) { } + + virtual ~ScaledLoss() { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + } + virtual void Evaluate(double, double*) const; + + private: + internal::scoped_ptr<const LossFunction> rho_; + const double a_; + const Ownership ownership_; + DISALLOW_COPY_AND_ASSIGN(ScaledLoss); +}; + +// Sometimes after the optimization problem has been constructed, we +// wish to mutate the scale of the loss function. For example, when +// performing estimation from data which has substantial outliers, +// convergence can be improved by starting out with a large scale, +// optimizing the problem and then reducing the scale. This can have +// better convergence behaviour than just using a loss function with a +// small scale. +// +// This templated class allows the user to implement a loss function +// whose scale can be mutated after an optimization problem has been +// constructed. +// +// Example usage +// +// Problem problem; +// +// // Add parameter blocks +// +// CostFunction* cost_function = +// new AutoDiffCostFunction < UW_Camera_Mapper, 2, 9, 3>( +// new UW_Camera_Mapper(data->observations[2*i + 0], +// data->observations[2*i + 1])); +// +// LossFunctionWrapper* loss_function(new HuberLoss(1.0), TAKE_OWNERSHIP); +// +// problem.AddResidualBlock(cost_function, loss_function, parameters); +// +// Solver::Options options; +// scoped_ptr<Solver::Summary> summary1(Solve(problem, options)); +// +// loss_function->Reset(new HuberLoss(1.0), TAKE_OWNERSHIP); +// +// scoped_ptr<Solver::Summary> summary2(Solve(problem, options)); +// +class LossFunctionWrapper : public LossFunction { + public: + LossFunctionWrapper(LossFunction* rho, Ownership ownership) + : rho_(rho), ownership_(ownership) { + } + + virtual ~LossFunctionWrapper() { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + } + + virtual void Evaluate(double sq_norm, double out[3]) const { + CHECK_NOTNULL(rho_.get()); + rho_->Evaluate(sq_norm, out); + } + + void Reset(LossFunction* rho, Ownership ownership) { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + rho_.reset(rho); + ownership_ = ownership; + } + + private: + internal::scoped_ptr<const LossFunction> rho_; + Ownership ownership_; + DISALLOW_COPY_AND_ASSIGN(LossFunctionWrapper); +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_LOSS_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/normal_prior.h b/extern/libmv/third_party/ceres/include/ceres/normal_prior.h new file mode 100644 index 00000000000..480a07474a7 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/normal_prior.h @@ -0,0 +1,75 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Cost term that implements a prior on a parameter block using a +// normal distribution. + +#ifndef CERES_PUBLIC_NORMAL_PRIOR_H_ +#define CERES_PUBLIC_NORMAL_PRIOR_H_ + +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" + +namespace ceres { + +// Implements a cost function of the form +// +// cost(x) = ||A(x - b)||^2 +// +// where, the matrix A and the vector b are fixed and x is the +// variable. In case the user is interested in implementing a cost +// function of the form +// +// cost(x) = (x - mu)^T S^{-1} (x - mu) +// +// where, mu is a vector and S is a covariance matrix, then, A = +// S^{-1/2}, i.e the matrix A is the square root of the inverse of the +// covariance, also known as the stiffness matrix. There are however +// no restrictions on the shape of A. It is free to be rectangular, +// which would be the case if the covariance matrix S is rank +// deficient. + +class NormalPrior: public CostFunction { + public: + // Check that the number of rows in the vector b are the same as the + // number of columns in the matrix A, crash otherwise. + NormalPrior(const Matrix& A, const Vector& b); + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const; + private: + Matrix A_; + Vector b_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_NORMAL_PRIOR_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/numeric_diff_cost_function.h b/extern/libmv/third_party/ceres/include/ceres/numeric_diff_cost_function.h new file mode 100644 index 00000000000..bbaefca5b6c --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/numeric_diff_cost_function.h @@ -0,0 +1,283 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Create CostFunctions as needed by the least squares framework with jacobians +// computed via numeric (a.k.a. finite) differentiation. For more details see +// http://en.wikipedia.org/wiki/Numerical_differentiation. +// +// To get a numerically differentiated cost function, define a subclass of +// CostFunction such that the Evaluate() function ignores the jacobian +// parameter. The numeric differentiation wrapper will fill in the jacobian +// parameter if nececssary by repeatedly calling the Evaluate() function with +// small changes to the appropriate parameters, and computing the slope. For +// performance, the numeric differentiation wrapper class is templated on the +// concrete cost function, even though it could be implemented only in terms of +// the virtual CostFunction interface. +// +// The numerically differentiated version of a cost function for a cost function +// can be constructed as follows: +// +// CostFunction* cost_function +// = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>( +// new MyCostFunction(...), TAKE_OWNERSHIP); +// +// where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8 +// respectively. Look at the tests for a more detailed example. +// +// The central difference method is considerably more accurate at the cost of +// twice as many function evaluations than forward difference. Consider using +// central differences begin with, and only after that works, trying forward +// difference to improve performance. +// +// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives. + +#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ + +#include <cstring> +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/sized_cost_function.h" +#include "ceres/types.h" + +namespace ceres { + +enum NumericDiffMethod { + CENTRAL, + FORWARD +}; + +// This is split from the main class because C++ doesn't allow partial template +// specializations for member functions. The alternative is to repeat the main +// class for differing numbers of parameters, which is also unfortunate. +template <typename CostFunctionNoJacobian, + int num_residuals, + int parameter_block_size, + int parameter_block, + NumericDiffMethod method> +struct Differencer { + // Mutates parameters but must restore them before return. + static bool EvaluateJacobianForParameterBlock( + const CostFunctionNoJacobian *function, + double const* residuals_at_eval_point, + double **parameters, + double **jacobians) { + using Eigen::Map; + using Eigen::Matrix; + using Eigen::RowMajor; + + typedef Matrix<double, num_residuals, 1> ResidualVector; + typedef Matrix<double, parameter_block_size, 1> ParameterVector; + typedef Matrix<double, num_residuals, parameter_block_size, RowMajor> + JacobianMatrix; + + Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block], + num_residuals, + parameter_block_size); + + // Mutate 1 element at a time and then restore. + Map<ParameterVector> x_plus_delta(parameters[parameter_block], + parameter_block_size); + ParameterVector x(x_plus_delta); + + // TODO(keir): Pick a smarter number! In theory a good choice is sqrt(eps) * + // x, which for doubles means about 1e-8 * x. However, I have found this + // number too optimistic. This number should be exposed for users to change. + const double kRelativeStepSize = 1e-6; + + ParameterVector step_size = x.array().abs() * kRelativeStepSize; + + // To handle cases where a parameter is exactly zero, instead use the mean + // step_size for the other dimensions. + double fallback_step_size = step_size.sum() / step_size.rows(); + if (fallback_step_size == 0.0) { + // If all the parameters are zero, there's no good answer. Take + // kRelativeStepSize as a guess and hope for the best. + fallback_step_size = kRelativeStepSize; + } + + // For each parameter in the parameter block, use finite differences to + // compute the derivative for that parameter. + for (int j = 0; j < parameter_block_size; ++j) { + if (step_size(j) == 0.0) { + // The parameter is exactly zero, so compromise and use the mean + // step_size from the other parameters. This can break in many cases, + // but it's hard to pick a good number without problem specific + // knowledge. + step_size(j) = fallback_step_size; + } + x_plus_delta(j) = x(j) + step_size(j); + + double residuals[num_residuals]; // NOLINT + if (!function->Evaluate(parameters, residuals, NULL)) { + // Something went wrong; bail. + return false; + } + + // Compute this column of the jacobian in 3 steps: + // 1. Store residuals for the forward part. + // 2. Subtract residuals for the backward (or 0) part. + // 3. Divide out the run. + parameter_jacobian.col(j) = + Map<const ResidualVector>(residuals, num_residuals); + + double one_over_h = 1 / step_size(j); + if (method == CENTRAL) { + // Compute the function on the other side of x(j). + x_plus_delta(j) = x(j) - step_size(j); + + if (!function->Evaluate(parameters, residuals, NULL)) { + // Something went wrong; bail. + return false; + } + parameter_jacobian.col(j) -= + Map<ResidualVector>(residuals, num_residuals, 1); + one_over_h /= 2; + } else { + // Forward difference only; reuse existing residuals evaluation. + parameter_jacobian.col(j) -= + Map<const ResidualVector>(residuals_at_eval_point, num_residuals); + } + x_plus_delta(j) = x(j); // Restore x_plus_delta. + + // Divide out the run to get slope. + parameter_jacobian.col(j) *= one_over_h; + } + return true; + } +}; + +// Prevent invalid instantiations. +template <typename CostFunctionNoJacobian, + int num_residuals, + int parameter_block, + NumericDiffMethod method> +struct Differencer<CostFunctionNoJacobian, + num_residuals, + 0 /* parameter_block_size */, + parameter_block, + method> { + static bool EvaluateJacobianForParameterBlock( + const CostFunctionNoJacobian *function, + double const* residuals_at_eval_point, + double **parameters, + double **jacobians) { + LOG(FATAL) << "Shouldn't get here."; + return true; + } +}; + +template <typename CostFunctionNoJacobian, + NumericDiffMethod method = CENTRAL, int M = 0, + int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5 = 0> +class NumericDiffCostFunction + : public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> { + public: + NumericDiffCostFunction(CostFunctionNoJacobian* function, + Ownership ownership) + : function_(function), ownership_(ownership) {} + + virtual ~NumericDiffCostFunction() { + if (ownership_ != TAKE_OWNERSHIP) { + function_.release(); + } + } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + // Get the function value (residuals) at the the point to evaluate. + bool success = function_->Evaluate(parameters, residuals, NULL); + if (!success) { + // Something went wrong; ignore the jacobian. + return false; + } + if (!jacobians) { + // Nothing to do; just forward. + return true; + } + + // Create a copy of the parameters which will get mutated. + const int kParametersSize = N0 + N1 + N2 + N3 + N4 + N5; + double parameters_copy[kParametersSize]; + double *parameters_references_copy[6]; + parameters_references_copy[0] = ¶meters_copy[0]; + parameters_references_copy[1] = ¶meters_copy[0] + N0; + parameters_references_copy[2] = ¶meters_copy[0] + N0 + N1; + parameters_references_copy[3] = ¶meters_copy[0] + N0 + N1 + N2; + parameters_references_copy[4] = ¶meters_copy[0] + N0 + N1 + N2 + N3; + parameters_references_copy[5] = + ¶meters_copy[0] + N0 + N1 + N2 + N3 + N4; + +#define COPY_PARAMETER_BLOCK(block) \ + if (N ## block) memcpy(parameters_references_copy[block], \ + parameters[block], \ + sizeof(double) * N ## block); // NOLINT + COPY_PARAMETER_BLOCK(0); + COPY_PARAMETER_BLOCK(1); + COPY_PARAMETER_BLOCK(2); + COPY_PARAMETER_BLOCK(3); + COPY_PARAMETER_BLOCK(4); + COPY_PARAMETER_BLOCK(5); +#undef COPY_PARAMETER_BLOCK + +#define EVALUATE_JACOBIAN_FOR_BLOCK(block) \ + if (N ## block && jacobians[block]) { \ + if (!Differencer<CostFunctionNoJacobian, /* NOLINT */ \ + M, \ + N ## block, \ + block, \ + method>::EvaluateJacobianForParameterBlock( \ + function_.get(), \ + residuals, \ + parameters_references_copy, \ + jacobians)) { \ + return false; \ + } \ + } + EVALUATE_JACOBIAN_FOR_BLOCK(0); + EVALUATE_JACOBIAN_FOR_BLOCK(1); + EVALUATE_JACOBIAN_FOR_BLOCK(2); + EVALUATE_JACOBIAN_FOR_BLOCK(3); + EVALUATE_JACOBIAN_FOR_BLOCK(4); + EVALUATE_JACOBIAN_FOR_BLOCK(5); +#undef EVALUATE_JACOBIAN_FOR_BLOCK + return true; + } + + private: + internal::scoped_ptr<CostFunctionNoJacobian> function_; + Ownership ownership_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/problem.h b/extern/libmv/third_party/ceres/include/ceres/problem.h new file mode 100644 index 00000000000..0ca61006bdb --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/problem.h @@ -0,0 +1,265 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) +// +// The Problem object is used to build and hold least squares problems. + +#ifndef CERES_PUBLIC_PROBLEM_H_ +#define CERES_PUBLIC_PROBLEM_H_ + +#include <cstddef> +#include <map> +#include <set> +#include <vector> + +#include <glog/logging.h> +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +class CostFunction; +class LossFunction; +class LocalParameterization; + +namespace internal { +class Preprocessor; +class ProblemImpl; +class ParameterBlock; +class ResidualBlock; +class SolverImpl; +} // namespace internal + +// A ResidualBlockId is a handle clients can use to delete residual +// blocks after creating them. They are opaque for any purposes other +// than that. +typedef const internal::ResidualBlock* ResidualBlockId; + +// A class to represent non-linear least squares problems. Such +// problems have a cost function that is a sum of error terms (known +// as "residuals"), where each residual is a function of some subset +// of the parameters. The cost function takes the form +// +// N 1 +// SUM --- loss( || r_i1, r_i2,..., r_ik ||^2 ), +// i=1 2 +// +// where +// +// r_ij is residual number i, component j; the residual is a +// function of some subset of the parameters x1...xk. For +// example, in a structure from motion problem a residual +// might be the difference between a measured point in an +// image and the reprojected position for the matching +// camera, point pair. The residual would have two +// components, error in x and error in y. +// +// loss(y) is the loss function; for example, squared error or +// Huber L1 loss. If loss(y) = y, then the cost function is +// non-robustified least squares. +// +// This class is specifically designed to address the important subset +// of "sparse" least squares problems, where each component of the +// residual depends only on a small number number of parameters, even +// though the total number of residuals and parameters may be very +// large. This property affords tremendous gains in scale, allowing +// efficient solving of large problems that are otherwise +// inaccessible. +// +// The canonical example of a sparse least squares problem is +// "structure-from-motion" (SFM), where the parameters are points and +// cameras, and residuals are reprojection errors. Typically a single +// residual will depend only on 9 parameters (3 for the point, 6 for +// the camera). +// +// To create a least squares problem, use the AddResidualBlock() and +// AddParameterBlock() methods, documented below. Here is an example least +// squares problem containing 3 parameter blocks of sizes 3, 4 and 5 +// respectively and two residual terms of size 2 and 6: +// +// double x1[] = { 1.0, 2.0, 3.0 }; +// double x2[] = { 1.0, 2.0, 3.0, 5.0 }; +// double x3[] = { 1.0, 2.0, 3.0, 6.0, 7.0 }; +// +// Problem problem; +// +// problem.AddResidualBlock(new MyUnaryCostFunction(...), x1); +// problem.AddResidualBlock(new MyBinaryCostFunction(...), x2, x3); +// +// Please see cost_function.h for details of the CostFunction object. +class Problem { + public: + struct Options { + Options() + : cost_function_ownership(TAKE_OWNERSHIP), + loss_function_ownership(TAKE_OWNERSHIP), + local_parameterization_ownership(TAKE_OWNERSHIP) {} + + // These flags control whether the Problem object owns the cost + // functions, loss functions, and parameterizations passed into + // the Problem. If set to TAKE_OWNERSHIP, then the problem object + // will delete the corresponding cost or loss functions on + // destruction. The destructor is careful to delete the pointers + // only once, since sharing cost/loss/parameterizations is + // allowed. + Ownership cost_function_ownership; + Ownership loss_function_ownership; + Ownership local_parameterization_ownership; + }; + + // The default constructor is equivalent to the + // invocation Problem(Problem::Options()). + Problem(); + explicit Problem(const Options& options); + + ~Problem(); + + // Add a residual block to the overall cost function. The cost + // function carries with it information about the sizes of the + // parameter blocks it expects. The function checks that these match + // the sizes of the parameter blocks listed in parameter_blocks. The + // program aborts if a mismatch is detected. loss_function can be + // NULL, in which case the cost of the term is just the squared norm + // of the residuals. + // + // The user has the option of explicitly adding the parameter blocks + // using AddParameterBlock. This causes additional correctness + // checking; however, AddResidualBlock implicitly adds the parameter + // blocks if they are not present, so calling AddParameterBlock + // explicitly is not required. + // + // The Problem object by default takes ownership of the + // cost_function and loss_function pointers. These objects remain + // live for the life of the Problem object. If the user wishes to + // keep control over the destruction of these objects, then they can + // do this by setting the corresponding enums in the Options struct. + // + // Note: Even though the Problem takes ownership of cost_function + // and loss_function, it does not preclude the user from re-using + // them in another residual block. The destructor takes care to call + // delete on each cost_function or loss_function pointer only once, + // regardless of how many residual blocks refer to them. + // + // Example usage: + // + // double x1[] = {1.0, 2.0, 3.0}; + // double x2[] = {1.0, 2.0, 5.0, 6.0}; + // double x3[] = {3.0, 6.0, 2.0, 5.0, 1.0}; + // + // Problem problem; + // + // problem.AddResidualBlock(new MyUnaryCostFunction(...), NULL, x1); + // problem.AddResidualBlock(new MyBinaryCostFunction(...), NULL, x2, x1); + // + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks); + + // Convenience methods for adding residuals with a small number of + // parameters. This is the common case. Instead of specifying the + // parameter block arguments as a vector, list them as pointers. + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5); + + // Add a parameter block with appropriate size to the problem. + // Repeated calls with the same arguments are ignored. Repeated + // calls with the same double pointer but a different size results + // in undefined behaviour. + void AddParameterBlock(double* values, int size); + + // Add a parameter block with appropriate size and parameterization + // to the problem. Repeated calls with the same arguments are + // ignored. Repeated calls with the same double pointer but a + // different size results in undefined behaviour. + void AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization); + + // Hold the indicated parameter block constant during optimization. + void SetParameterBlockConstant(double* values); + + // Allow the indicated parameter to vary during optimization. + void SetParameterBlockVariable(double* values); + + // Set the local parameterization for one of the parameter blocks. + // The local_parameterization is owned by the Problem by default. It + // is acceptable to set the same parameterization for multiple + // parameters; the destructor is careful to delete local + // parameterizations only once. The local parameterization can only + // be set once per parameter, and cannot be changed once set. + void SetParameterization(double* values, + LocalParameterization* local_parameterization); + + // Number of parameter blocks in the problem. Always equals + // parameter_blocks().size() and parameter_block_sizes().size(). + int NumParameterBlocks() const; + + // The size of the parameter vector obtained by summing over the + // sizes of all the parameter blocks. + int NumParameters() const; + + // Number of residual blocks in the problem. Always equals + // residual_blocks().size(). + int NumResidualBlocks() const; + + // The size of the residual vector obtained by summing over the + // sizes of all of the residual blocks. + int NumResiduals() const; + + private: + friend class internal::SolverImpl; + internal::scoped_ptr<internal::ProblemImpl> problem_impl_; + DISALLOW_COPY_AND_ASSIGN(Problem); +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_PROBLEM_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/rotation.h b/extern/libmv/third_party/ceres/include/ceres/rotation.h new file mode 100644 index 00000000000..e4227e78b9a --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/rotation.h @@ -0,0 +1,526 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Templated functions for manipulating rotations. The templated +// functions are useful when implementing functors for automatic +// differentiation. +// +// In the following, the Quaternions are laid out as 4-vectors, thus: +// +// q[0] scalar part. +// q[1] coefficient of i. +// q[2] coefficient of j. +// q[3] coefficient of k. +// +// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j. + +#ifndef CERES_PUBLIC_ROTATION_H_ +#define CERES_PUBLIC_ROTATION_H_ + +#include <algorithm> +#include <cmath> + +namespace ceres { + +// Convert a value in combined axis-angle representation to a quaternion. +// The value angle_axis is a triple whose norm is an angle in radians, +// and whose direction is aligned with the axis of rotation, +// and quaternion is a 4-tuple that will contain the resulting quaternion. +// The implementation may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template<typename T> +void AngleAxisToQuaternion(T const* angle_axis, T* quaternion); + +// Convert a quaternion to the equivalent combined axis-angle representation. +// The value quaternion must be a unit quaternion - it is not normalized first, +// and angle_axis will be filled with a value whose norm is the angle of +// rotation in radians, and whose direction is the axis of rotation. +// The implemention may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template<typename T> +void QuaternionToAngleAxis(T const* quaternion, T* angle_axis); + +// Conversions between 3x3 rotation matrix (in column major order) and +// axis-angle rotation representations. Templated for use with +// autodifferentiation. +template <typename T> +void RotationMatrixToAngleAxis(T const * R, T * angle_axis); +template <typename T> +void AngleAxisToRotationMatrix(T const * angle_axis, T * R); + +// Conversions between 3x3 rotation matrix (in row major order) and +// Euler angle (in degrees) rotation representations. +// +// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} +// axes, respectively. They are applied in that same order, so the +// total rotation R is Rz * Ry * Rx. +template <typename T> +void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); + +// Convert a 4-vector to a 3x3 scaled rotation matrix. +// +// The choice of rotation is such that the quaternion [1 0 0 0] goes to an +// identity matrix and for small a, b, c the quaternion [1 a b c] goes to +// the matrix +// +// [ 0 -c b ] +// I + 2 [ c 0 -a ] + higher order terms +// [ -b a 0 ] +// +// which corresponds to a Rodrigues approximation, the last matrix being +// the cross-product matrix of [a b c]. Together with the property that +// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R. +// +// The rotation matrix is row-major. +// +// No normalization of the quaternion is performed, i.e. +// R = ||q||^2 * Q, where Q is an orthonormal matrix +// such that det(Q) = 1 and Q*Q' = I +template <typename T> inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); + +// Same as above except that the rotation matrix is normalized by the +// Frobenius norm, so that R * R' = I (and det(R) = 1). +template <typename T> inline +void QuaternionToRotation(const T q[4], T R[3 * 3]); + +// Rotates a point pt by a quaternion q: +// +// result = R(q) * pt +// +// Assumes the quaternion is unit norm. This assumption allows us to +// write the transform as (something)*pt + pt, as is clear from the +// formula below. If you pass in a quaternion with |q|^2 = 2 then you +// WILL NOT get back 2 times the result you get for a unit quaternion. +template <typename T> inline +void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// With this function you do not need to assume that q has unit norm. +// It does assume that the norm is non-zero. +template <typename T> inline +void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// zw = z * w, where * is the Quaternion product between 4 vectors. +template<typename T> inline +void QuaternionProduct(const T z[4], const T w[4], T zw[4]); + +// xy = x cross y; +template<typename T> inline +void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); + +template<typename T> inline +T DotProduct(const T x[3], const T y[3]); + +// y = R(angle_axis) * x; +template<typename T> inline +void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); + +// --- IMPLEMENTATION + +// Duplicate rather than decorate every use of cmath with _USE_MATH_CONSTANTS. +// Necessitated by Windows. +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#define CERES_NEED_M_PI_UNDEF +#endif + +template<typename T> +inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { + const T &a0 = angle_axis[0]; + const T &a1 = angle_axis[1]; + const T &a2 = angle_axis[2]; + const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2; + + // For points not at the origin, the full conversion is numerically stable. + if (theta_squared > T(0.0)) { + const T theta = sqrt(theta_squared); + const T half_theta = theta * T(0.5); + const T k = sin(half_theta) / theta; + quaternion[0] = cos(half_theta); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } else { + // At the origin, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(0.5); + quaternion[0] = T(1.0); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } +} + +template<typename T> +inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { + const T &q1 = quaternion[1]; + const T &q2 = quaternion[2]; + const T &q3 = quaternion[3]; + const T sin_squared = q1 * q1 + q2 * q2 + q3 * q3; + + // For quaternions representing non-zero rotation, the conversion + // is numerically stable. + if (sin_squared > T(0.0)) { + const T sin_theta = sqrt(sin_squared); + const T k = T(2.0) * atan2(sin_theta, quaternion[0]) / sin_theta; + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } else { + // For zero rotation, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(2.0); + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } +} + +// The conversion of a rotation matrix to the angle-axis form is +// numerically problematic when then rotation angle is close to zero +// or to Pi. The following implementation detects when these two cases +// occurs and deals with them by taking code paths that are guaranteed +// to not perform division by a small number. +template <typename T> +inline void RotationMatrixToAngleAxis(const T * R, T * angle_axis) { + // x = k * 2 * sin(theta), where k is the axis of rotation. + angle_axis[0] = R[5] - R[7]; + angle_axis[1] = R[6] - R[2]; + angle_axis[2] = R[1] - R[3]; + + static const T kOne = T(1.0); + static const T kTwo = T(2.0); + + // Since the right hand side may give numbers just above 1.0 or + // below -1.0 leading to atan misbehaving, we threshold. + T costheta = std::min(std::max((R[0] + R[4] + R[8] - kOne) / kTwo, + T(-1.0)), + kOne); + + // sqrt is guaranteed to give non-negative results, so we only + // threshold above. + T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] + + angle_axis[1] * angle_axis[1] + + angle_axis[2] * angle_axis[2]) / kTwo, + kOne); + + // Use the arctan2 to get the right sign on theta + const T theta = atan2(sintheta, costheta); + + // Case 1: sin(theta) is large enough, so dividing by it is not a + // problem. We do not use abs here, because while jets.h imports + // std::abs into the namespace, here in this file, abs resolves to + // the int version of the function, which returns zero always. + // + // We use a threshold much larger then the machine epsilon, because + // if sin(theta) is small, not only do we risk overflow but even if + // that does not occur, just dividing by a small number will result + // in numerical garbage. So we play it safe. + static const double kThreshold = 1e-12; + if ((sintheta > kThreshold) || (sintheta < -kThreshold)) { + const T r = theta / (kTwo * sintheta); + for (int i = 0; i < 3; ++i) { + angle_axis[i] *= r; + } + return; + } + + // Case 2: theta ~ 0, means sin(theta) ~ theta to a good + // approximation. + if (costheta > 0) { + const T kHalf = T(0.5); + for (int i = 0; i < 3; ++i) { + angle_axis[i] *= kHalf; + } + return; + } + + // Case 3: theta ~ pi, this is the hard case. Since theta is large, + // and sin(theta) is small. Dividing by theta by sin(theta) will + // either give an overflow or worse still numerically meaningless + // results. Thus we use an alternate more complicated formula + // here. + + // Since cos(theta) is negative, division by (1-cos(theta)) cannot + // overflow. + const T inv_one_minus_costheta = kOne / (kOne - costheta); + + // We now compute the absolute value of coordinates of the axis + // vector using the diagonal entries of R. To resolve the sign of + // these entries, we compare the sign of angle_axis[i]*sin(theta) + // with the sign of sin(theta). If they are the same, then + // angle_axis[i] should be positive, otherwise negative. + for (int i = 0; i < 3; ++i) { + angle_axis[i] = theta * sqrt((R[i*4] - costheta) * inv_one_minus_costheta); + if (((sintheta < 0) && (angle_axis[i] > 0)) || + ((sintheta > 0) && (angle_axis[i] < 0))) { + angle_axis[i] = -angle_axis[i]; + } + } +} + +template <typename T> +inline void AngleAxisToRotationMatrix(const T * angle_axis, T * R) { + static const T kOne = T(1.0); + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > 0.0) { + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + const T theta = sqrt(theta2); + const T wx = angle_axis[0] / theta; + const T wy = angle_axis[1] / theta; + const T wz = angle_axis[2] / theta; + + const T costheta = cos(theta); + const T sintheta = sin(theta); + + R[0] = costheta + wx*wx*(kOne - costheta); + R[1] = wz*sintheta + wx*wy*(kOne - costheta); + R[2] = -wy*sintheta + wx*wz*(kOne - costheta); + R[3] = wx*wy*(kOne - costheta) - wz*sintheta; + R[4] = costheta + wy*wy*(kOne - costheta); + R[5] = wx*sintheta + wy*wz*(kOne - costheta); + R[6] = wy*sintheta + wx*wz*(kOne - costheta); + R[7] = -wx*sintheta + wy*wz*(kOne - costheta); + R[8] = costheta + wz*wz*(kOne - costheta); + } else { + // At zero, we switch to using the first order Taylor expansion. + R[0] = kOne; + R[1] = -angle_axis[2]; + R[2] = angle_axis[1]; + R[3] = angle_axis[2]; + R[4] = kOne; + R[5] = -angle_axis[0]; + R[6] = -angle_axis[1]; + R[7] = angle_axis[0]; + R[8] = kOne; + } +} + +template <typename T> +inline void EulerAnglesToRotationMatrix(const T* euler, + const int row_stride, + T* R) { + const T degrees_to_radians(M_PI / 180.0); + + const T pitch(euler[0] * degrees_to_radians); + const T roll(euler[1] * degrees_to_radians); + const T yaw(euler[2] * degrees_to_radians); + + const T c1 = cos(yaw); + const T s1 = sin(yaw); + const T c2 = cos(roll); + const T s2 = sin(roll); + const T c3 = cos(pitch); + const T s3 = sin(pitch); + + // Rows of the rotation matrix. + T* R1 = R; + T* R2 = R1 + row_stride; + T* R3 = R2 + row_stride; + + R1[0] = c1*c2; + R1[1] = -s1*c3 + c1*s2*s3; + R1[2] = s1*s3 + c1*s2*c3; + + R2[0] = s1*c2; + R2[1] = c1*c3 + s1*s2*s3; + R2[2] = -c1*s3 + s1*s2*c3; + + R3[0] = -s2; + R3[1] = c2*s3; + R3[2] = c2*c3; +} + +template <typename T> inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { + // Make convenient names for elements of q. + T a = q[0]; + T b = q[1]; + T c = q[2]; + T d = q[3]; + // This is not to eliminate common sub-expression, but to + // make the lines shorter so that they fit in 80 columns! + T aa = a * a; + T ab = a * b; + T ac = a * c; + T ad = a * d; + T bb = b * b; + T bc = b * c; + T bd = b * d; + T cc = c * c; + T cd = c * d; + T dd = d * d; + + R[0] = aa + bb - cc - dd; R[1] = T(2) * (bc - ad); R[2] = T(2) * (ac + bd); // NOLINT + R[3] = T(2) * (ad + bc); R[4] = aa - bb + cc - dd; R[5] = T(2) * (cd - ab); // NOLINT + R[6] = T(2) * (bd - ac); R[7] = T(2) * (ab + cd); R[8] = aa - bb - cc + dd; // NOLINT +} + +template <typename T> inline +void QuaternionToRotation(const T q[4], T R[3 * 3]) { + QuaternionToScaledRotation(q, R); + + T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; + CHECK_NE(normalizer, T(0)); + normalizer = T(1) / normalizer; + + for (int i = 0; i < 9; ++i) { + R[i] *= normalizer; + } +} + +template <typename T> inline +void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + const T t2 = q[0] * q[1]; + const T t3 = q[0] * q[2]; + const T t4 = q[0] * q[3]; + const T t5 = -q[1] * q[1]; + const T t6 = q[1] * q[2]; + const T t7 = q[1] * q[3]; + const T t8 = -q[2] * q[2]; + const T t9 = q[2] * q[3]; + const T t1 = -q[3] * q[3]; + result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT + result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT + result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT +} + + +template <typename T> inline +void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + // 'scale' is 1 / norm(q). + const T scale = T(1) / sqrt(q[0] * q[0] + + q[1] * q[1] + + q[2] * q[2] + + q[3] * q[3]); + + // Make unit-norm version of q. + const T unit[4] = { + scale * q[0], + scale * q[1], + scale * q[2], + scale * q[3], + }; + + UnitQuaternionRotatePoint(unit, pt, result); +} + +template<typename T> inline +void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { + zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; + zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; + zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; + zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; +} + +// xy = x cross y; +template<typename T> inline +void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { + x_cross_y[0] = x[1] * y[2] - x[2] * y[1]; + x_cross_y[1] = x[2] * y[0] - x[0] * y[2]; + x_cross_y[2] = x[0] * y[1] - x[1] * y[0]; +} + +template<typename T> inline +T DotProduct(const T x[3], const T y[3]) { + return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]); +} + +template<typename T> inline +void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { + T w[3]; + T sintheta; + T costheta; + + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > 0.0) { + // Away from zero, use the rodriguez formula + // + // result = pt costheta + + // (w x pt) * sintheta + + // w (w . pt) (1 - costheta) + // + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + // + const T theta = sqrt(theta2); + w[0] = angle_axis[0] / theta; + w[1] = angle_axis[1] / theta; + w[2] = angle_axis[2] / theta; + costheta = cos(theta); + sintheta = sin(theta); + T w_cross_pt[3]; + CrossProduct(w, pt, w_cross_pt); + T w_dot_pt = DotProduct(w, pt); + for (int i = 0; i < 3; ++i) { + result[i] = pt[i] * costheta + + w_cross_pt[i] * sintheta + + w[i] * (T(1.0) - costheta) * w_dot_pt; + } + } else { + // Near zero, the first order Taylor approximation of the rotation + // matrix R corresponding to a vector w and angle w is + // + // R = I + hat(w) * sin(theta) + // + // But sintheta ~ theta and theta * w = angle_axis, which gives us + // + // R = I + hat(w) + // + // and actually performing multiplication with the point pt, gives us + // R * pt = pt + w x pt. + // + // Switching to the Taylor expansion at zero helps avoid all sorts + // of numerical nastiness. + T w_cross_pt[3]; + CrossProduct(angle_axis, pt, w_cross_pt); + for (int i = 0; i < 3; ++i) { + result[i] = pt[i] + w_cross_pt[i]; + } + } +} + +} // namespace ceres + +// Clean define pollution. +#ifdef CERES_NEED_M_PI_UNDEF +#undef CERES_NEED_M_PI_UNDEF +#undef M_PI +#endif + +#endif // CERES_PUBLIC_ROTATION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/sized_cost_function.h b/extern/libmv/third_party/ceres/include/ceres/sized_cost_function.h new file mode 100644 index 00000000000..2894a9fba5c --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/sized_cost_function.h @@ -0,0 +1,88 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A convenience class for cost functions which are statically sized. +// Compared to the dynamically-sized base class, this reduces boilerplate. +// +// The kNumResiduals template parameter can be a constant such as 2 or 5, or it +// can be ceres::DYNAMIC. If kNumResiduals is ceres::DYNAMIC, then subclasses +// are responsible for calling set_num_residuals() at runtime. + +#ifndef CERES_PUBLIC_SIZED_COST_FUNCTION_H_ +#define CERES_PUBLIC_SIZED_COST_FUNCTION_H_ + +#include <glog/logging.h> +#include "ceres/types.h" +#include "ceres/cost_function.h" + +namespace ceres { + +template<int kNumResiduals, + int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5 = 0> +class SizedCostFunction : public CostFunction { + public: + SizedCostFunction() { + // Sanity checking. + CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC) + << "Cost functions must have at least one residual block."; + + CHECK_GT(N0, 0) + << "Cost functions must have at least one parameter block."; + CHECK((!N1 && !N2 && !N3 && !N4 && !N5) || + ((N1 > 0) && !N2 && !N3 && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0))) + << "Zero block cannot precede a non-zero block. Block sizes are " + << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " + << N3 << ", " << N4 << ", " << N5; + + set_num_residuals(kNumResiduals); + +#define ADD_PARAMETER_BLOCK(N) \ + if (N) mutable_parameter_block_sizes()->push_back(N); + ADD_PARAMETER_BLOCK(N0); + ADD_PARAMETER_BLOCK(N1); + ADD_PARAMETER_BLOCK(N2); + ADD_PARAMETER_BLOCK(N3); + ADD_PARAMETER_BLOCK(N4); + ADD_PARAMETER_BLOCK(N5); +#undef ADD_PARAMETER_BLOCK + } + + virtual ~SizedCostFunction() { } + + // Subclasses must implement Evaluate(). +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_SIZED_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/solver.h b/extern/libmv/third_party/ceres/include/ceres/solver.h new file mode 100644 index 00000000000..bd669272023 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/solver.h @@ -0,0 +1,376 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_SOLVER_H_ +#define CERES_PUBLIC_SOLVER_H_ + +#include <cmath> +#include <string> +#include <vector> + +#include "ceres/iteration_callback.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { + +class Problem; + +// Interface for non-linear least squares solvers. +class Solver { + public: + virtual ~Solver(); + + // The options structure contains, not surprisingly, options that control how + // the solver operates. The defaults should be suitable for a wide range of + // problems; however, better performance is often obtainable with tweaking. + // + // The constants are defined inside types.h + struct Options { + // Default constructor that sets up a generic sparse problem. + Options() { + minimizer_type = LEVENBERG_MARQUARDT; + max_num_iterations = 50; + max_solver_time_sec = 1.0e9; + num_threads = 1; + tau = 1e-4; + min_relative_decrease = 1e-3; + function_tolerance = 1e-6; + gradient_tolerance = 1e-10; + parameter_tolerance = 1e-8; +#ifndef CERES_NO_SUITESPARSE + linear_solver_type = SPARSE_NORMAL_CHOLESKY; +#else + linear_solver_type = DENSE_QR; +#endif // CERES_NO_SUITESPARSE + preconditioner_type = JACOBI; + num_linear_solver_threads = 1; + num_eliminate_blocks = 0; + ordering_type = NATURAL; + linear_solver_min_num_iterations = 1; + linear_solver_max_num_iterations = 500; + eta = 1e-1; + jacobi_scaling = true; + logging_type = PER_MINIMIZER_ITERATION; + minimizer_progress_to_stdout = false; + return_initial_residuals = false; + return_final_residuals = false; + lsqp_dump_directory = "/tmp"; + lsqp_dump_format_type = TEXTFILE; + crash_and_dump_lsqp_on_failure = false; + check_gradients = false; + gradient_check_relative_precision = 1e-8; + numeric_derivative_relative_step_size = 1e-6; + update_state_every_iteration = false; + } + + // Minimizer options ---------------------------------------- + + MinimizerType minimizer_type; + + // Maximum number of iterations for the minimizer to run for. + int max_num_iterations; + + // Maximum time for which the minimizer should run for. + double max_solver_time_sec; + + // Number of threads used by Ceres for evaluating the cost and + // jacobians. + int num_threads; + + // For Levenberg-Marquardt, the initial value for the + // regularizer. This is the inversely related to the size of the + // initial trust region. + double tau; + + // For trust region methods, this is lower threshold for the + // relative decrease before a step is accepted. + double min_relative_decrease; + + // Minimizer terminates when + // + // (new_cost - old_cost) < function_tolerance * old_cost; + // + double function_tolerance; + + // Minimizer terminates when + // + // max_i |gradient_i| < gradient_tolerance * max_i|initial_gradient_i| + // + // This value should typically be 1e-4 * function_tolerance. + double gradient_tolerance; + + // Minimizer terminates when + // + // |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance) + // + double parameter_tolerance; + + // Linear least squares solver options ------------------------------------- + + LinearSolverType linear_solver_type; + + // Type of preconditioner to use with the iterative linear solvers. + PreconditionerType preconditioner_type; + + // Number of threads used by Ceres to solve the Newton + // step. Currently only the SPARSE_SCHUR solver is capable of + // using this setting. + int num_linear_solver_threads; + + // For Schur reduction based methods, the first 0 to num blocks are + // eliminated using the Schur reduction. For example, when solving + // traditional structure from motion problems where the parameters are in + // two classes (cameras and points) then num_eliminate_blocks would be the + // number of points. + // + // This parameter is used in conjunction with the ordering. + // Applies to: Preprocessor and linear least squares solver. + int num_eliminate_blocks; + + // Internally Ceres reorders the parameter blocks to help the + // various linear solvers. This parameter allows the user to + // influence the re-ordering strategy used. For structure from + // motion problems use SCHUR, for other problems NATURAL (default) + // is a good choice. In case you wish to specify your own ordering + // scheme, for example in conjunction with num_eliminate_blocks, + // use USER. + OrderingType ordering_type; + + // The ordering of the parameter blocks. The solver pays attention + // to it if the ordering_type is set to USER and the vector is + // non-empty. + vector<double*> ordering; + + + // Minimum number of iterations for which the linear solver should + // run, even if the convergence criterion is satisfied. + int linear_solver_min_num_iterations; + + // Maximum number of iterations for which the linear solver should + // run. If the solver does not converge in less than + // linear_solver_max_num_iterations, then it returns + // MAX_ITERATIONS, as its termination type. + int linear_solver_max_num_iterations; + + // Forcing sequence parameter. The truncated Newton solver uses + // this number to control the relative accuracy with which the + // Newton step is computed. + // + // This constant is passed to ConjugateGradientsSolver which uses + // it to terminate the iterations when + // + // (Q_i - Q_{i-1})/Q_i < eta/i + double eta; + + // Normalize the jacobian using Jacobi scaling before calling + // the linear least squares solver. + bool jacobi_scaling; + + // Logging options --------------------------------------------------------- + + LoggingType logging_type; + + // By default the Minimizer progress is logged to VLOG(1), which + // is sent to STDERR depending on the vlog level. If this flag is + // set to true, and logging_type is not SILENT, the logging output + // is sent to STDOUT. + bool minimizer_progress_to_stdout; + + bool return_initial_residuals; + bool return_final_residuals; + + // List of iterations at which the optimizer should dump the + // linear least squares problem to disk. Useful for testing and + // benchmarking. If empty (default), no problems are dumped. + // + // This is ignored if protocol buffers are disabled. + vector<int> lsqp_iterations_to_dump; + string lsqp_dump_directory; + DumpFormatType lsqp_dump_format_type; + + // Dump the linear least squares problem to disk if the minimizer + // fails due to NUMERICAL_FAILURE and crash the process. This flag + // is useful for generating debugging information. The problem is + // dumped in a file whose name is determined by + // Solver::Options::lsqp_dump_format. + // + // Note: This requires a version of Ceres built with protocol buffers. + bool crash_and_dump_lsqp_on_failure; + + // Finite differences options ---------------------------------------------- + + // Check all jacobians computed by each residual block with finite + // differences. This is expensive since it involves computing the + // derivative by normal means (e.g. user specified, autodiff, + // etc), then also computing it using finite differences. The + // results are compared, and if they differ substantially, details + // are printed to the log. + bool check_gradients; + + // Relative precision to check for in the gradient checker. If the + // relative difference between an element in a jacobian exceeds + // this number, then the jacobian for that cost term is dumped. + double gradient_check_relative_precision; + + // Relative shift used for taking numeric derivatives. For finite + // differencing, each dimension is evaluated at slightly shifted + // values; for the case of central difference, this is what gets + // evaluated: + // + // delta = numeric_derivative_relative_step_size; + // f_initial = f(x) + // f_forward = f((1 + delta) * x) + // f_backward = f((1 - delta) * x) + // + // The finite differencing is done along each dimension. The + // reason to use a relative (rather than absolute) step size is + // that this way, numeric differentation works for functions where + // the arguments are typically large (e.g. 1e9) and when the + // values are small (e.g. 1e-5). It is possible to construct + // "torture cases" which break this finite difference heuristic, + // but they do not come up often in practice. + // + // TODO(keir): Pick a smarter number than the default above! In + // theory a good choice is sqrt(eps) * x, which for doubles means + // about 1e-8 * x. However, I have found this number too + // optimistic. This number should be exposed for users to change. + double numeric_derivative_relative_step_size; + + // If true, the user's parameter blocks are updated at the end of + // every Minimizer iteration, otherwise they are updated when the + // Minimizer terminates. This is useful if, for example, the user + // wishes to visualize the state of the optimization every + // iteration. + bool update_state_every_iteration; + + // Callbacks that are executed at the end of each iteration of the + // Minimizer. They are executed in the order that they are + // specified in this vector. By default, parameter blocks are + // updated only at the end of the optimization, i.e when the + // Minimizer terminates. This behaviour is controlled by + // update_state_every_variable. If the user wishes to have access + // to the update parameter blocks when his/her callbacks are + // executed, then set update_state_every_iteration to true. + // + // The solver does NOT take ownership of these pointers. + vector<IterationCallback*> callbacks; + }; + + struct Summary { + Summary(); + + // A brief one line description of the state of the solver after + // termination. + string BriefReport() const; + + // A full multiline description of the state of the solver after + // termination. + string FullReport() const; + + // Minimizer summary ------------------------------------------------- + SolverTerminationType termination_type; + + // If the solver did not run, or there was a failure, a + // description of the error. + string error; + + // Cost of the problem before and after the optimization. See + // problem.h for definition of the cost of a problem. + double initial_cost; + double final_cost; + + // The part of the total cost that comes from residual blocks that + // were held fixed by the preprocessor because all the parameter + // blocks that they depend on were fixed. + double fixed_cost; + + // Residuals before and after the optimization. Each vector + // contains problem.NumResiduals() elements. Residuals are in the + // same order in which they were added to the problem object when + // constructing this problem. + vector<double> initial_residuals; + vector<double> final_residuals; + + vector<IterationSummary> iterations; + + int num_successful_steps; + int num_unsuccessful_steps; + + double preprocessor_time_in_seconds; + double minimizer_time_in_seconds; + double total_time_in_seconds; + + // Preprocessor summary. + int num_parameter_blocks; + int num_parameters; + int num_residual_blocks; + int num_residuals; + + int num_parameter_blocks_reduced; + int num_parameters_reduced; + int num_residual_blocks_reduced; + int num_residuals_reduced; + + int num_eliminate_blocks_given; + int num_eliminate_blocks_used; + + int num_threads_given; + int num_threads_used; + + int num_linear_solver_threads_given; + int num_linear_solver_threads_used; + + LinearSolverType linear_solver_type_given; + LinearSolverType linear_solver_type_used; + + PreconditionerType preconditioner_type; + OrderingType ordering_type; + }; + + // Once a least squares problem has been built, this function takes + // the problem and optimizes it based on the values of the options + // parameters. Upon return, a detailed summary of the work performed + // by the preprocessor, the non-linear minmizer and the linear + // solver are reported in the summary object. + virtual void Solve(const Options& options, + Problem* problem, + Solver::Summary* summary); +}; + +// Helper function which avoids going through the interface. +void Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary); + +} // namespace ceres + +#endif // CERES_PUBLIC_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/types.h b/extern/libmv/third_party/ceres/include/ceres/types.h new file mode 100644 index 00000000000..a30c79029ac --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/types.h @@ -0,0 +1,258 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Enums and other top level class definitions. +// +// Note: internal/types.cc defines stringification routines for some +// of these enums. Please update those routines if you extend or +// remove enums from here. + +#ifndef CERES_PUBLIC_TYPES_H_ +#define CERES_PUBLIC_TYPES_H_ + +namespace ceres { + +// Basic integer types. These typedefs are in the Ceres namespace to avoid +// conflicts with other packages having similar typedefs. +typedef short int16; +typedef int int32; + +// Argument type used in interfaces that can optionally take ownership +// of a passed in argument. If TAKE_OWNERSHIP is passed, the called +// object takes ownership of the pointer argument, and will call +// delete on it upon completion. +enum Ownership { + DO_NOT_TAKE_OWNERSHIP, + TAKE_OWNERSHIP +}; + +// TODO(keir): Considerably expand the explanations of each solver type. +enum LinearSolverType { + // These solvers are for general rectangular systems formed from the + // normal equations A'A x = A'b. They are direct solvers and do not + // assume any special problem structure. + + // Solve the normal equations using a sparse cholesky solver; based + // on CHOLMOD. + SPARSE_NORMAL_CHOLESKY, + + // Solve the normal equations using a dense QR solver; based on + // Eigen. + DENSE_QR, + + // Specialized solvers, specific to problems with a generalized + // bi-partitite structure. + + // Solves the reduced linear system using a dense Cholesky solver; + // based on Eigen. + DENSE_SCHUR, + + // Solves the reduced linear system using a sparse Cholesky solver; + // based on CHOLMOD. + SPARSE_SCHUR, + + // Solves the reduced linear system using Conjugate Gradients, based + // on a new Ceres implementation. Suitable for large scale + // problems. + ITERATIVE_SCHUR, + + // Conjugate gradients on the normal equations. + CGNR +}; + +enum PreconditionerType { + // Trivial preconditioner - the identity matrix. + IDENTITY, + + // Block diagonal of the Gauss-Newton Hessian. + JACOBI, + + // Block diagonal of the Schur complement. This preconditioner may + // only be used with the ITERATIVE_SCHUR solver. Requires + // SuiteSparse/CHOLMOD. + SCHUR_JACOBI, + + // Visibility clustering based preconditioners. + // + // These preconditioners are well suited for Structure from Motion + // problems, particularly problems arising from community photo + // collections. These preconditioners use the visibility structure + // of the scene to determine the sparsity structure of the + // preconditioner. Requires SuiteSparse/CHOLMOD. + CLUSTER_JACOBI, + CLUSTER_TRIDIAGONAL +}; + +enum LinearSolverTerminationType { + // Termination criterion was met. For factorization based solvers + // the tolerance is assumed to be zero. Any user provided values are + // ignored. + TOLERANCE, + + // Solver ran for max_num_iterations and terminated before the + // termination tolerance could be satified. + MAX_ITERATIONS, + + // Solver is stuck and further iterations will not result in any + // measurable progress. + STAGNATION, + + // Solver failed. Solver was terminated due to numerical errors. The + // exact cause of failure depends on the particular solver being + // used. + FAILURE +}; + +enum OrderingType { + // The order in which the parameter blocks were defined. + NATURAL, + + // Use the ordering specificed in the vector ordering. + USER, + + // Automatically figure out the best ordering to use the schur + // complement based solver. + SCHUR +}; + +// Logging options +// The options get progressively noisier. +enum LoggingType { + SILENT, + PER_MINIMIZER_ITERATION +}; + +enum MinimizerType { + LEVENBERG_MARQUARDT +}; + +enum SolverTerminationType { + // The minimizer did not run at all; usually due to errors in the user's + // Problem or the solver options. + DID_NOT_RUN, + + // The solver ran for maximum number of iterations specified by the + // user, but none of the convergence criterion specified by the user + // were met. + NO_CONVERGENCE, + + // Minimizer terminated because + // (new_cost - old_cost) < function_tolerance * old_cost; + FUNCTION_TOLERANCE, + + // Minimizer terminated because + // max_i |gradient_i| < gradient_tolerance * max_i|initial_gradient_i| + GRADIENT_TOLERANCE, + + // Minimized terminated because + // |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance) + PARAMETER_TOLERANCE, + + // The minimizer terminated because it encountered a numerical error + // that it could not recover from. + NUMERICAL_FAILURE, + + // Using an IterationCallback object, user code can control the + // minimizer. The following enums indicate that the user code was + // responsible for termination. + + // User's IterationCallback returned SOLVER_ABORT. + USER_ABORT, + + // User's IterationCallback returned SOLVER_TERMINATE_SUCCESSFULLY + USER_SUCCESS +}; + +// Enums used by the IterationCallback instances to indicate to the +// solver whether it should continue solving, the user detected an +// error or the solution is good enough and the solver should +// terminate. +enum CallbackReturnType { + // Continue solving to next iteration. + SOLVER_CONTINUE, + + // Terminate solver, and do not update the parameter blocks upon + // return. Unless the user has set + // Solver:Options:::update_state_every_iteration, in which case the + // state would have been updated every iteration + // anyways. Solver::Summary::termination_type is set to USER_ABORT. + SOLVER_ABORT, + + // Terminate solver, update state and + // return. Solver::Summary::termination_type is set to USER_SUCCESS. + SOLVER_TERMINATE_SUCCESSFULLY +}; + +// The format in which linear least squares problems should be logged +// when Solver::Options::lsqp_iterations_to_dump is non-empty. +enum DumpFormatType { + // Print the linear least squares problem in a human readable format + // to stderr. The Jacobian is printed as a dense matrix. The vectors + // D, x and f are printed as dense vectors. This should only be used + // for small problems. + CONSOLE, + + // Write out the linear least squares problem to the directory + // pointed to by Solver::Options::lsqp_dump_directory as a protocol + // buffer. linear_least_squares_problems.h/cc contains routines for + // loading these problems. For details on the on disk format used, + // see matrix.proto. The files are named lm_iteration_???.lsqp. + PROTOBUF, + + // Write out the linear least squares problem to the directory + // pointed to by Solver::Options::lsqp_dump_directory as text files + // which can be read into MATLAB/Octave. The Jacobian is dumped as a + // text file containing (i,j,s) triplets, the vectors D, x and f are + // dumped as text files containing a list of their values. + // + // A MATLAB/octave script called lm_iteration_???.m is also output, + // which can be used to parse and load the problem into memory. + TEXTFILE +}; + +// For SizedCostFunction and AutoDiffCostFunction, DYNAMIC can be specified for +// the number of residuals. If specified, then the number of residuas for that +// cost function can vary at runtime. +enum DimensionType { + DYNAMIC = -1 +}; + +const char* LinearSolverTypeToString(LinearSolverType type); +const char* PreconditionerTypeToString(PreconditionerType type); +const char* LinearSolverTerminationTypeToString( + LinearSolverTerminationType type); +const char* OrderingTypeToString(OrderingType type); +const char* SolverTerminationTypeToString(SolverTerminationType type); + +bool IsSchurType(LinearSolverType type); + +} // namespace ceres + +#endif // CERES_PUBLIC_TYPES_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.cc b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.cc new file mode 100644 index 00000000000..05e63eb560b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.cc @@ -0,0 +1,73 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/block_evaluate_preparer.h" + +#include <vector> +#include "ceres/block_sparse_matrix.h" +#include "ceres/casts.h" +#include "ceres/parameter_block.h" +#include "ceres/residual_block.h" +#include "ceres/sparse_matrix.h" + +namespace ceres { +namespace internal { + +void BlockEvaluatePreparer::Init(int** jacobian_layout) { + jacobian_layout_ = jacobian_layout; +} + +// Point the jacobian blocks directly into the block sparse matrix. +void BlockEvaluatePreparer::Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians) const { + CHECK(jacobian != NULL); + double* jacobian_values = + down_cast<BlockSparseMatrix*>(jacobian)->mutable_values(); + + const int* jacobian_block_offset = jacobian_layout_[residual_block_index]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + if (!residual_block->parameter_blocks()[j]->IsConstant()) { + jacobians[j] = jacobian_values + *jacobian_block_offset; + + // The jacobian_block_offset can't be indexed with 'j' since the code + // that creates the layout strips out any blocks for inactive + // parameters. Instead, bump the pointer for active parameters only. + jacobian_block_offset++; + } else { + jacobians[j] = NULL; + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.h b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.h new file mode 100644 index 00000000000..a7869311e6e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.h @@ -0,0 +1,67 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A evaluate preparer which puts jacobian the evaluated jacobian blocks +// directly into their final resting place in an overall block sparse matrix. +// The evaluator takes care to avoid evaluating the jacobian for fixed +// parameters. + +#ifndef CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ +#define CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ + +namespace ceres { +namespace internal { + +class ResidualBlock; +class SparseMatrix; + +class BlockEvaluatePreparer { + public: + // Using Init() instead of a constructor allows for allocating this structure + // with new[]. This is because C++ doesn't allow passing arguments to objects + // constructed with new[] (as opposed to plain 'new'). + void Init(int** jacobian_layout); + + // EvaluatePreparer interface + + // Point the jacobian blocks directly into the block sparse matrix. + void Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians) const; + + private: + int const* const* jacobian_layout_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc new file mode 100644 index 00000000000..1a5001f9c71 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc @@ -0,0 +1,136 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/block_jacobi_preconditioner.h" + +#include "Eigen/Cholesky" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/casts.h" +#include "ceres/integral_types.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +BlockJacobiPreconditioner::BlockJacobiPreconditioner( + const LinearOperator& A) + : block_structure_( + *(down_cast<const BlockSparseMatrix*>(&A)->block_structure())), + num_rows_(A.num_rows()) { + // Calculate the amount of storage needed. + int storage_needed = 0; + for (int c = 0; c < block_structure_.cols.size(); ++c) { + int size = block_structure_.cols[c].size; + storage_needed += size * size; + } + + // Size the offsets and storage. + blocks_.resize(block_structure_.cols.size()); + block_storage_.resize(storage_needed); + + // Put pointers to the storage in the offsets. + double* block_cursor = &block_storage_[0]; + for (int c = 0; c < block_structure_.cols.size(); ++c) { + int size = block_structure_.cols[c].size; + blocks_[c] = block_cursor; + block_cursor += size * size; + } +} + +BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { +} + +void BlockJacobiPreconditioner::Update(const LinearOperator& matrix, const double* D) { + const BlockSparseMatrix& A = *(down_cast<const BlockSparseMatrix*>(&matrix)); + const CompressedRowBlockStructure* bs = A.block_structure(); + + // Compute the diagonal blocks by block inner products. + std::fill(block_storage_.begin(), block_storage_.end(), 0.0); + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_size = bs->rows[r].block.size; + const vector<Cell>& cells = bs->rows[r].cells; + const double* row_values = A.RowBlockValues(r); + for (int c = 0; c < cells.size(); ++c) { + const int col_block_size = bs->cols[cells[c].block_id].size; + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + + MatrixRef(blocks_[cells[c].block_id], + col_block_size, + col_block_size).noalias() += m.transpose() * m; + + // TODO(keir): Figure out when the below expression is actually faster + // than doing the full rank update. The issue is that for smaller sizes, + // the rankUpdate() function is slower than the full product done above. + // + // On the typical bundling problems, the above product is ~5% faster. + // + // MatrixRef(blocks_[cells[c].block_id], + // col_block_size, + // col_block_size).selfadjointView<Eigen::Upper>().rankUpdate(m); + // + } + } + + // Add the diagonal and invert each block. + for (int c = 0; c < bs->cols.size(); ++c) { + const int size = block_structure_.cols[c].size; + const int position = block_structure_.cols[c].position; + MatrixRef block(blocks_[c], size, size); + + if (D != NULL) { + block.diagonal() += ConstVectorRef(D + position, size).array().square().matrix(); + } + + block = block.selfadjointView<Eigen::Upper>() + .ldlt() + .solve(Matrix::Identity(size, size)); + } +} + +void BlockJacobiPreconditioner::RightMultiply(const double* x, double* y) const { + for (int c = 0; c < block_structure_.cols.size(); ++c) { + const int size = block_structure_.cols[c].size; + const int position = block_structure_.cols[c].position; + ConstMatrixRef D(blocks_[c], size, size); + ConstVectorRef x_block(x + position, size); + VectorRef y_block(y + position, size); + y_block += D * x_block; + } +} + +void BlockJacobiPreconditioner::LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h new file mode 100644 index 00000000000..91cfeddb688 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h @@ -0,0 +1,84 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ +#define CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ + +#include <vector> +#include "ceres/linear_operator.h" + +namespace ceres { +namespace internal { + +class CompressedRowBlockStructure; +class LinearOperator; +class SparseMatrix; + +// A block Jacobi preconditioner. This is intended for use with conjugate +// gradients, or other iterative symmetric solvers. To use the preconditioner, +// create one by passing a BlockSparseMatrix as the linear operator "A" to the +// constructor. This fixes the sparsity pattern to the pattern of the matrix +// A^TA. +// +// Before each use of the preconditioner in a solve with conjugate gradients, +// update the matrix by running Update(A, D). The values of the matrix A are +// inspected to construct the preconditioner. The vector D is applied as the +// D^TD diagonal term. +class BlockJacobiPreconditioner : public LinearOperator { + public: + // A must remain valid while the BlockJacobiPreconditioner is. + BlockJacobiPreconditioner(const LinearOperator& A); + virtual ~BlockJacobiPreconditioner(); + + // Update the preconditioner with the values found in A. The sparsity pattern + // must match that of the A passed to the constructor. D is a vector that + // must have the same number of rows as A, and is applied as a diagonal in + // addition to the block diagonals of A. + void Update(const LinearOperator& A, const double* D); + + // LinearOperator interface. + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_rows_; } + + private: + std::vector<double*> blocks_; + std::vector<double> block_storage_; + int num_rows_; + + // The block structure of the matrix this preconditioner is for (e.g. J). + const CompressedRowBlockStructure& block_structure_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.cc b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.cc new file mode 100644 index 00000000000..52a58bb43a6 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.cc @@ -0,0 +1,209 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/block_jacobian_writer.h" + +#include "ceres/block_evaluate_preparer.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { +namespace { + +// Given the residual block ordering, build a lookup table to determine which +// per-parameter jacobian goes where in the overall program jacobian. +// +// Since we expect to use a Schur type linear solver to solve the LM step, take +// extra care to place the E blocks and the F blocks contiguously. E blocks are +// the first num_eliminate_blocks parameter blocks as indicated by the parameter +// block ordering. The remaining parameter blocks are the F blocks. +// +// TODO(keir): Consider if we should use a boolean for each parameter block +// instead of num_eliminate_blocks. +void BuildJacobianLayout(const Program& program, + int num_eliminate_blocks, + vector<int*>* jacobian_layout, + vector<int>* jacobian_layout_storage) { + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + + // Iterate over all the active residual blocks and determine how many E blocks + // are there. This will determine where the F blocks start in the jacobian + // matrix. Also compute the number of jacobian blocks. + int f_block_pos = 0; + int num_jacobian_blocks = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Advance f_block_pos over each E block for this residual. + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + // Only count blocks for active parameters. + num_jacobian_blocks++; + if (parameter_block->index() < num_eliminate_blocks) { + f_block_pos += num_residuals * parameter_block->LocalSize(); + } + } + } + } + + // We now know that the E blocks are laid out starting at zero, and the F + // blocks are laid out starting at f_block_pos. Iterate over the residual + // blocks again, and this time fill the jacobian_layout array with the + // position information. + + jacobian_layout->resize(program.NumResidualBlocks()); + jacobian_layout_storage->resize(num_jacobian_blocks); + + int e_block_pos = 0; + int* jacobian_pos = &(*jacobian_layout_storage)[0]; + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + (*jacobian_layout)[i] = jacobian_pos; + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + const int parameter_block_index = parameter_block->index(); + if (parameter_block->IsConstant()) { + continue; + } + const int jacobian_block_size = + num_residuals * parameter_block->LocalSize(); + if (parameter_block_index < num_eliminate_blocks) { + *jacobian_pos = e_block_pos; + e_block_pos += jacobian_block_size; + } else { + *jacobian_pos = f_block_pos; + f_block_pos += jacobian_block_size; + } + jacobian_pos++; + } + } +} + +} // namespace + +BlockJacobianWriter::BlockJacobianWriter(const Evaluator::Options& options, + Program* program) + : program_(program) { + CHECK_GE(options.num_eliminate_blocks, 0) + << "num_eliminate_blocks must be greater than 0."; + + BuildJacobianLayout(*program, + options.num_eliminate_blocks, + &jacobian_layout_, + &jacobian_layout_storage_); +} + +// Create evaluate prepareres that point directly into the final jacobian. This +// makes the final Write() a nop. +BlockEvaluatePreparer* BlockJacobianWriter::CreateEvaluatePreparers( + int num_threads) { + BlockEvaluatePreparer* preparers = new BlockEvaluatePreparer[num_threads]; + for (int i = 0; i < num_threads; i++) { + preparers[i].Init(&jacobian_layout_[0]); + } + return preparers; +} + +SparseMatrix* BlockJacobianWriter::CreateJacobian() const { + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + + const vector<ParameterBlock*>& parameter_blocks = + program_->parameter_blocks(); + + // Construct the column blocks. + bs->cols.resize(parameter_blocks.size()); + for (int i = 0, cursor = 0; i < parameter_blocks.size(); ++i) { + CHECK_NE(parameter_blocks[i]->index(), -1); + CHECK(!parameter_blocks[i]->IsConstant()); + bs->cols[i].size = parameter_blocks[i]->LocalSize(); + bs->cols[i].position = cursor; + cursor += bs->cols[i].size; + } + + // Construct the cells in each row. + const vector<ResidualBlock*>& residual_blocks = + program_->residual_blocks(); + int row_block_position = 0; + bs->rows.resize(residual_blocks.size()); + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + CompressedRow* row = &bs->rows[i]; + + row->block.size = residual_block->NumResiduals(); + row->block.position = row_block_position; + row_block_position += row->block.size; + + // Size the row by the number of active parameters in this residual. + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + int num_active_parameter_blocks = 0; + for (int j = 0; j < num_parameter_blocks; ++j) { + if (residual_block->parameter_blocks()[j]->index() != -1) { + num_active_parameter_blocks++; + } + } + row->cells.resize(num_active_parameter_blocks); + + // Add layout information for the active parameters in this row. + for (int j = 0, k = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + Cell& cell = row->cells[k]; + cell.block_id = parameter_block->index(); + cell.position = jacobian_layout_[i][k]; + + // Only increment k for active parameters, since there is only layout + // information for active parameters. + k++; + } + } + + sort(row->cells.begin(), row->cells.end(), CellLessThan); + } + + BlockSparseMatrix* jacobian = new BlockSparseMatrix(bs); + CHECK_NOTNULL(jacobian); + return jacobian; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.h b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.h new file mode 100644 index 00000000000..140c7211129 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.h @@ -0,0 +1,127 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A jacobian writer that writes to block sparse matrices. The "writer" name is +// misleading, since the Write() operation on the block jacobian writer does not +// write anything. Instead, the Prepare() method on the BlockEvaluatePreparers +// makes a jacobians array which has direct pointers into the block sparse +// jacobian. When the cost function is evaluated, the jacobian blocks get placed +// directly in their final location. + +#ifndef CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ + +#include <vector> +#include "ceres/evaluator.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class BlockEvaluatePreparer; +class Program; +class SparseMatrix; + +class BlockJacobianWriter { + public: + BlockJacobianWriter(const Evaluator::Options& options, + Program* program); + + // JacobianWriter interface. + + // Create evaluate prepareres that point directly into the final jacobian. + // This makes the final Write() a nop. + BlockEvaluatePreparer* CreateEvaluatePreparers(int num_threads); + + SparseMatrix* CreateJacobian() const; + + void Write(int /* residual_id */, + int /* residual_offset */, + double** /* jacobians */, + SparseMatrix* /* jacobian */) { + // This is a noop since the blocks were written directly into their final + // position by the outside evaluate call, thanks to the jacobians array + // prepared by the BlockEvaluatePreparers. + } + + private: + Program* program_; + + // Stores the position of each residual / parameter jacobian. + // + // The block sparse matrix that this writer writes to is stored as a set of + // contiguos dense blocks, one after each other; see BlockSparseMatrix. The + // "double* values_" member of the block sparse matrix contains all of these + // blocks. Given a pointer to the first element of a block and the size of + // that block, it's possible to write to it. + // + // In the case of a block sparse jacobian, the jacobian writer needs a way to + // find the offset in the values_ array of each residual/parameter jacobian + // block. + // + // That is the purpose of jacobian_layout_. + // + // In particular, jacobian_layout_[i][j] is the offset in the values_ array of + // the derivative of residual block i with respect to the parameter block at + // active argument position j. + // + // The active qualifier means that non-active parameters do not count. Care + // must be taken when indexing into jacobian_layout_ to account for this. + // Consider a single residual example: + // + // r(x, y, z) + // + // with r in R^3, x in R^4, y in R^2, and z in R^5. + // Take y as a constant (non-active) parameter. + // Take r as residual number 0. + // + // In this case, the active arguments are only (x, z), so the active argument + // position for x is 0, and the active argument position for z is 1. This is + // similar to thinking of r as taking only 2 parameters: + // + // r(x, z) + // + // There are only 2 jacobian blocks: dr/dx and dr/dz. jacobian_layout_ would + // have the following contents: + // + // jacobian_layout_[0] = { 0, 12 } + // + // which indicates that dr/dx is located at values_[0], and dr/dz is at + // values_[12]. See BlockEvaluatePreparer::Prepare()'s comments about 'j'. + vector<int*> jacobian_layout_; + + // The pointers in jacobian_layout_ point directly into this vector. + vector<int> jacobian_layout_storage_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.cc new file mode 100644 index 00000000000..2afaf5e2ea2 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.cc @@ -0,0 +1,83 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_dense_matrix.h" + +#include <vector> +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessDenseMatrix::BlockRandomAccessDenseMatrix( + const vector<int>& blocks) { + block_layout_.resize(blocks.size(), 0); + num_rows_ = 0; + for (int i = 0; i < blocks.size(); ++i) { + block_layout_[i] = num_rows_; + num_rows_ += blocks[i]; + } + + values_.reset(new double[num_rows_ * num_rows_]); + CHECK_NOTNULL(values_.get()); + cell_info_.values = values_.get(); + SetZero(); +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +BlockRandomAccessDenseMatrix::~BlockRandomAccessDenseMatrix() { +} + +CellInfo* BlockRandomAccessDenseMatrix::GetCell(const int row_block_id, + const int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) { + *row = block_layout_[row_block_id]; + *col = block_layout_[col_block_id]; + *row_stride = num_rows_; + *col_stride = num_rows_; + return &cell_info_; +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +void BlockRandomAccessDenseMatrix::SetZero() { + if (num_rows_) { + VectorRef(values_.get(), num_rows_ * num_rows_).setZero(); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.h new file mode 100644 index 00000000000..3a0096209f7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.h @@ -0,0 +1,98 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ + +#include "ceres/block_random_access_matrix.h" + +#include <vector> + +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// A square block random accessible matrix with the same row and +// column block structure. All cells are stored in the same single +// array, so that its also accessible as a dense matrix of size +// num_rows x num_cols. +// +// This class is NOT thread safe. Since all n^2 cells are stored, +// GetCell never returns NULL for any (row_block_id, col_block_id) +// pair. +// +// ReturnCell is a nop. +class BlockRandomAccessDenseMatrix : public BlockRandomAccessMatrix { + public: + // blocks is a vector of block sizes. The resulting matrix has + // blocks.size() * blocks.size() cells. + explicit BlockRandomAccessDenseMatrix(const vector<int>& blocks); + + // The destructor is not thread safe. It assumes that no one is + // modifying any cells when the matrix is being destroyed. + virtual ~BlockRandomAccessDenseMatrix(); + + // BlockRandomAccessMatrix interface. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride); + + // This is not a thread safe method, it assumes that no cell is + // locked. + virtual void SetZero(); + + // Since the matrix is square with the same row and column block + // structure, num_rows() = num_cols(). + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_rows_; } + + // The underlying matrix storing the cells. + const double* values() const { return values_.get(); } + double* mutable_values() { return values_.get(); } + + private: + CellInfo cell_info_; + int num_rows_; + vector<int> block_layout_; + scoped_array<double> values_; + + DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessDenseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.cc new file mode 100644 index 00000000000..58fe4a10de3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_matrix.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessMatrix::~BlockRandomAccessMatrix() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.h new file mode 100644 index 00000000000..f398af3be87 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.h @@ -0,0 +1,132 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Interface for matrices that allow block based random access. + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ + +#include "ceres/mutex.h" + +namespace ceres { +namespace internal { + +// A matrix implementing the BlockRandomAccessMatrix interface is a +// matrix whose rows and columns are divided into blocks. For example +// the matrix A: +// +// 3 4 5 +// A = 5 [c_11 c_12 c_13] +// 4 [c_21 c_22 c_23] +// +// has row blocks of size 5 and 4, and column blocks of size 3, 4 and +// 5. It has six cells corresponding to the six row-column block +// combinations. +// +// BlockRandomAccessMatrix objects provide access to cells c_ij using +// the GetCell method. when a cell is present, GetCell will return a +// CellInfo object containing a pointer to an array which contains the +// cell as a submatrix and a mutex that guards this submatrix. If the +// user is accessing the matrix concurrently, it is his responsibility +// to use the mutex to exclude other writers from writing to the cell +// concurrently. +// +// There is no requirement that all cells be present, i.e. the matrix +// itself can be block sparse. When a cell is not present, the GetCell +// method will return a NULL pointer. +// +// There is no requirement about how the cells are stored beyond that +// form a dense submatrix of a larger dense matrix. Like everywhere +// else in Ceres, RowMajor storage assumed. +// +// Example usage: +// +// BlockRandomAccessMatrix* A = new BlockRandomAccessMatrixSubClass(...) +// +// int row, col, row_stride, col_stride; +// CellInfo* cell = A->GetCell(row_block_id, col_block_id, +// &row, &col, +// &row_stride, &col_stride); +// +// if (cell != NULL) { +// MatrixRef m(cell->values, row_stride, col_stride); +// MutexLock l(&cell->m); +// m.block(row, col, row_block_size, col_block_size) = ... +// } + +// Structure to carry a pointer to the array containing a cell and the +// Mutex guarding it. +struct CellInfo { + CellInfo() + : values(NULL) { + } + + explicit CellInfo(double* ptr) + : values(ptr) { + } + + double* values; + Mutex m; +}; + +class BlockRandomAccessMatrix { + public: + virtual ~BlockRandomAccessMatrix(); + + // If the cell (row_block_id, col_block_id) is present, then return + // a CellInfo with a pointer to the dense matrix containing it, + // otherwise return NULL. The dense matrix containing this cell has + // size row_stride, col_stride and the cell is located at position + // (row, col) within this matrix. + // + // The size of the cell is row_block_size x col_block_size is + // assumed known to the caller. row_block_size less than or equal to + // row_stride and col_block_size is upper bounded by col_stride. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) = 0; + + // Zero out the values of the array. The structure of the matrix + // (size and sparsity) is preserved. + virtual void SetZero() = 0; + + // Number of scalar rows and columns in the matrix, i.e the sum of + // all row blocks and column block sizes respectively. + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc new file mode 100644 index 00000000000..c496fcd13de --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc @@ -0,0 +1,158 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_sparse_matrix.h" + +#include <algorithm> +#include <set> +#include <utility> +#include <vector> +#include <glog/logging.h> +#include "ceres/mutex.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( + const vector<int>& blocks, + const set<pair<int, int> >& block_pairs) + : kMaxRowBlocks(10 * 1000 * 1000), + blocks_(blocks) { + CHECK_LT(blocks.size(), kMaxRowBlocks); + + // Build the row/column layout vector and count the number of scalar + // rows/columns. + int num_cols = 0; + vector<int> col_layout; + for (int i = 0; i < blocks_.size(); ++i) { + col_layout.push_back(num_cols); + num_cols += blocks_[i]; + } + + // Count the number of scalar non-zero entries and build the layout + // object for looking into the values array of the + // TripletSparseMatrix. + int num_nonzeros = 0; + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_size = blocks_[it->first]; + const int col_block_size = blocks_[it->second]; + num_nonzeros += row_block_size * col_block_size; + } + + VLOG(1) << "Matrix Size [" << num_cols + << "," << num_cols + << "] " << num_nonzeros; + + tsm_.reset(new TripletSparseMatrix(num_cols, num_cols, num_nonzeros)); + tsm_->set_num_nonzeros(num_nonzeros); + int* rows = tsm_->mutable_rows(); + int* cols = tsm_->mutable_cols(); + double* values = tsm_->mutable_values(); + + int pos = 0; + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_size = blocks_[it->first]; + const int col_block_size = blocks_[it->second]; + layout_[IntPairToLong(it->first, it->second)] = + new CellInfo(values + pos); + pos += row_block_size * col_block_size; + } + + // Fill the sparsity pattern of the underlying matrix. + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_id = it->first; + const int col_block_id = it->second; + const int row_block_size = blocks_[row_block_id]; + const int col_block_size = blocks_[col_block_id]; + int pos = + layout_[IntPairToLong(row_block_id, col_block_id)]->values - values; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c, ++pos) { + rows[pos] = col_layout[row_block_id] + r; + cols[pos] = col_layout[col_block_id] + c; + values[pos] = 1.0; + DCHECK_LT(rows[pos], tsm_->num_rows()); + DCHECK_LT(cols[pos], tsm_->num_rows()); + } + } + } +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +BlockRandomAccessSparseMatrix::~BlockRandomAccessSparseMatrix() { + for (LayoutType::iterator it = layout_.begin(); + it != layout_.end(); + ++it) { + delete it->second; + } +} + +CellInfo* BlockRandomAccessSparseMatrix::GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) { + const LayoutType::iterator it = + layout_.find(IntPairToLong(row_block_id, col_block_id)); + if (it == layout_.end()) { + return NULL; + } + + // Each cell is stored contiguously as its own little dense matrix. + *row = 0; + *col = 0; + *row_stride = blocks_[row_block_id]; + *col_stride = blocks_[col_block_id]; + return it->second; +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +void BlockRandomAccessSparseMatrix::SetZero() { + if (tsm_->num_nonzeros()) { + VectorRef(tsm_->mutable_values(), + tsm_->num_nonzeros()).setZero(); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h new file mode 100644 index 00000000000..12613c3daa0 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h @@ -0,0 +1,109 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/mutex.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/collections_port.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// A threaf safe square block sparse implementation of +// BlockRandomAccessMatrix. Internally a TripletSparseMatrix is used +// for doing the actual storage. This class augments this matrix with +// an unordered_map that allows random read/write access. +class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { + public: + // blocks is an array of block sizes. block_pairs is a set of + // <row_block_id, col_block_id> pairs to identify the non-zero cells + // of this matrix. + BlockRandomAccessSparseMatrix(const vector<int>& blocks, + const set<pair<int, int> >& block_pairs); + + // The destructor is not thread safe. It assumes that no one is + // modifying any cells when the matrix is being destroyed. + virtual ~BlockRandomAccessSparseMatrix(); + + // BlockRandomAccessMatrix Interface. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride); + + // This is not a thread safe method, it assumes that no cell is + // locked. + virtual void SetZero(); + virtual bool IsThreadSafe() const { return true; } + + // Since the matrix is square, num_rows() == num_cols(). + virtual int num_rows() const { return tsm_->num_rows(); } + virtual int num_cols() const { return tsm_->num_cols(); } + + // Access to the underlying matrix object. + const TripletSparseMatrix* matrix() const { return tsm_.get(); } + TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } + + private: + long int IntPairToLong(int a, int b) { + return a * kMaxRowBlocks + b; + } + + const int kMaxRowBlocks; + // row/column block sizes. + const vector<int> blocks_; + + // A mapping from <row_block_id, col_block_id> to the position in + // the values array of tsm_ where the block is stored. + typedef HashMap<long int, CellInfo* > LayoutType; + LayoutType layout_; + + // The underlying matrix object which actually stores the cells. + scoped_ptr<TripletSparseMatrix> tsm_; + + DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc new file mode 100644 index 00000000000..7dd395e2975 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc @@ -0,0 +1,286 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_sparse_matrix.h" + +#include <cstddef> +#include <algorithm> +#include <vector> +#include <glog/logging.h> +#include "ceres/block_structure.h" +#include "ceres/matrix_proto.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +BlockSparseMatrix::~BlockSparseMatrix() {} + +BlockSparseMatrix::BlockSparseMatrix( + CompressedRowBlockStructure* block_structure) + : num_rows_(0), + num_cols_(0), + num_nonzeros_(0), + values_(NULL), + block_structure_(block_structure) { + CHECK_NOTNULL(block_structure_.get()); + + // Count the number of columns in the matrix. + for (int i = 0; i < block_structure_->cols.size(); ++i) { + num_cols_ += block_structure_->cols[i].size; + } + + // Count the number of non-zero entries and the number of rows in + // the matrix. + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + num_rows_ += row_block_size; + + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + num_nonzeros_ += col_block_size * row_block_size; + } + } + + CHECK_GE(num_rows_, 0); + CHECK_GE(num_cols_, 0); + CHECK_GE(num_nonzeros_, 0); + VLOG(2) << "Allocating values array with " + << num_nonzeros_ * sizeof(double) << " bytes."; // NOLINT + values_.reset(new double[num_nonzeros_]); + CHECK_NOTNULL(values_.get()); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +BlockSparseMatrix::BlockSparseMatrix(const SparseMatrixProto& outer_proto) { + CHECK(outer_proto.has_block_matrix()); + + const BlockSparseMatrixProto& proto = outer_proto.block_matrix(); + CHECK(proto.has_num_rows()); + CHECK(proto.has_num_cols()); + CHECK_EQ(proto.num_nonzeros(), proto.values_size()); + + num_rows_ = proto.num_rows(); + num_cols_ = proto.num_cols(); + num_nonzeros_ = proto.num_nonzeros(); + + // Copy out the values into *this. + values_.reset(new double[num_nonzeros_]); + for (int i = 0; i < proto.num_nonzeros(); ++i) { + values_[i] = proto.values(i); + } + + // Create the block structure according to the proto. + block_structure_.reset(new CompressedRowBlockStructure); + ProtoToBlockStructure(proto.block_structure(), block_structure_.get()); +} +#endif + +void BlockSparseMatrix::SetZero() { + fill(values_.get(), values_.get() + num_nonzeros_, 0.0); +} + +void BlockSparseMatrix::RightMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + VectorRef yref(y + row_block_pos, row_block_size); + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + ConstVectorRef xref(x + col_block_pos, col_block_size); + MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + yref += m.lazyProduct(xref); + } + } +} + +void BlockSparseMatrix::LeftMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const ConstVectorRef xref(x + row_block_pos, row_block_size); + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + VectorRef yref(y + col_block_pos, col_block_size); + MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + yref += m.transpose().lazyProduct(xref); + } + } +} + +void BlockSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + VectorRef(x, num_cols_).setZero(); + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + const MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + VectorRef(x + col_block_pos, col_block_size) += m.colwise().squaredNorm(); + } + } +} + +void BlockSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + m *= ConstVectorRef(scale + col_block_pos, col_block_size).asDiagonal(); + } + } +} + +void BlockSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + CHECK_NOTNULL(dense_matrix); + + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + Matrix& m = *dense_matrix; + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + m.block(row_block_pos, col_block_pos, row_block_size, col_block_size) + += MatrixRef(values_.get() + jac_pos, row_block_size, col_block_size); + } + } +} + +void BlockSparseMatrix::ToTripletSparseMatrix( + TripletSparseMatrix* matrix) const { + CHECK_NOTNULL(matrix); + + matrix->Reserve(num_nonzeros_); + matrix->Resize(num_rows_, num_cols_); + matrix->SetZero(); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c, ++jac_pos) { + matrix->mutable_rows()[jac_pos] = row_block_pos + r; + matrix->mutable_cols()[jac_pos] = col_block_pos + c; + matrix->mutable_values()[jac_pos] = values_[jac_pos]; + } + } + } + } + matrix->set_num_nonzeros(num_nonzeros_); +} + +// Return a pointer to the block structure. We continue to hold +// ownership of the object though. +const CompressedRowBlockStructure* BlockSparseMatrix::block_structure() + const { + return block_structure_.get(); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void BlockSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { + outer_proto->Clear(); + + BlockSparseMatrixProto* proto = outer_proto->mutable_block_matrix(); + proto->set_num_rows(num_rows_); + proto->set_num_cols(num_cols_); + proto->set_num_nonzeros(num_nonzeros_); + for (int i = 0; i < num_nonzeros_; ++i) { + proto->add_values(values_[i]); + } + BlockStructureToProto(*block_structure_, proto->mutable_block_structure()); +} +#endif + +void BlockSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + for (int i = 0; i < block_structure_->rows.size(); ++i) { + const int row_block_pos = block_structure_->rows[i].block.position; + const int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + const int col_block_id = cells[j].block_id; + const int col_block_size = block_structure_->cols[col_block_id].size; + const int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c) { + fprintf(file, "% 10d % 10d %17f\n", + row_block_pos + r, + col_block_pos + c, + values_[jac_pos++]); + } + } + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h new file mode 100644 index 00000000000..f71446e8f58 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h @@ -0,0 +1,144 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Implementation of the SparseMatrix interface for block sparse +// matrices. + +#ifndef CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ + +#include "ceres/block_structure.h" +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; +class TripletSparseMatrix; + +// A further extension of the SparseMatrix interface to support block-oriented +// matrices. The key addition is the RowBlockValues() accessor, which enables +// the lazy block sparse matrix implementation. +class BlockSparseMatrixBase : public SparseMatrix { + public: + BlockSparseMatrixBase() {} + virtual ~BlockSparseMatrixBase() {} + + // Convert this matrix into a triplet sparse matrix. + virtual void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const = 0; + + // Returns a pointer to the block structure. Does not transfer + // ownership. + virtual const CompressedRowBlockStructure* block_structure() const = 0; + + // Returns a pointer to a row of the matrix. The returned array is only valid + // until the next call to RowBlockValues. The caller does not own the result. + // + // The returned array is laid out such that cells on the specified row are + // contiguous in the returned array, though neighbouring cells in row order + // may not be contiguous in the row values. The cell values for cell + // (row_block, cell_block) are found at offset + // + // block_structure()->rows[row_block].cells[cell_block].position + // + virtual const double* RowBlockValues(int row_block_index) const = 0; + + private: + DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrixBase); +}; + +// This class implements the SparseMatrix interface for storing and +// manipulating block sparse matrices. The block structure is stored +// in the CompressedRowBlockStructure object and one is needed to +// initialize the matrix. For details on how the blocks structure of +// the matrix is stored please see the documentation +// +// internal/ceres/block_structure.h +// +class BlockSparseMatrix : public BlockSparseMatrixBase { + public: + // Construct a block sparse matrix with a fully initialized + // CompressedRowBlockStructure objected. The matrix takes over + // ownership of this object and destroys it upon destruction. + // + // TODO(sameeragarwal): Add a function which will validate legal + // CompressedRowBlockStructure objects. + explicit BlockSparseMatrix(CompressedRowBlockStructure* block_structure); + + // Construct a block sparse matrix from a protocol buffer. +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit BlockSparseMatrix(const SparseMatrixProto& proto); +#endif + + BlockSparseMatrix(); + virtual ~BlockSparseMatrix(); + + // Implementation of SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto* proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_cols_; } + virtual int num_nonzeros() const { return num_nonzeros_; } + virtual const double* values() const { return values_.get(); } + virtual double* mutable_values() { return values_.get(); } + + // Implementation of BlockSparseMatrixBase interface. + virtual void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const; + virtual const CompressedRowBlockStructure* block_structure() const; + virtual const double* RowBlockValues(int row_block_index) const { + return values_.get(); + } + + private: + int num_rows_; + int num_cols_; + int max_num_nonzeros_; + int num_nonzeros_; + scoped_array<double> values_; + scoped_ptr<CompressedRowBlockStructure> block_structure_; + DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc b/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc new file mode 100644 index 00000000000..5add4f3b94d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc @@ -0,0 +1,92 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_structure.h" +#include "ceres/matrix_proto.h" + +namespace ceres { +namespace internal { + +bool CellLessThan(const Cell& lhs, const Cell& rhs) { + return (lhs.block_id < rhs.block_id); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void ProtoToBlockStructure(const BlockStructureProto &proto, + CompressedRowBlockStructure *block_structure) { + // Decode the column blocks. + block_structure->cols.resize(proto.cols_size()); + for (int i = 0; i < proto.cols_size(); ++i) { + block_structure->cols[i].size = proto.cols(i).size(); + block_structure->cols[i].position = + proto.cols(i).position(); + } + // Decode the row structure. + block_structure->rows.resize(proto.rows_size()); + for (int i = 0; i < proto.rows_size(); ++i) { + const CompressedRowProto &row = proto.rows(i); + block_structure->rows[i].block.size = row.block().size(); + block_structure->rows[i].block.position = row.block().position(); + + // Copy the cells within the row. + block_structure->rows[i].cells.resize(row.cells_size()); + for (int j = 0; j < row.cells_size(); ++j) { + const CellProto &cell = row.cells(j); + block_structure->rows[i].cells[j].block_id = cell.block_id(); + block_structure->rows[i].cells[j].position = cell.position(); + } + } +} + +void BlockStructureToProto(const CompressedRowBlockStructure &block_structure, + BlockStructureProto *proto) { + // Encode the column blocks. + for (int i = 0; i < block_structure.cols.size(); ++i) { + BlockProto *block = proto->add_cols(); + block->set_size(block_structure.cols[i].size); + block->set_position(block_structure.cols[i].position); + } + // Encode the row structure. + for (int i = 0; i < block_structure.rows.size(); ++i) { + CompressedRowProto *row = proto->add_rows(); + BlockProto *block = row->mutable_block(); + block->set_size(block_structure.rows[i].block.size); + block->set_position(block_structure.rows[i].block.position); + for (int j = 0; j < block_structure.rows[i].cells.size(); ++j) { + CellProto *cell = row->add_cells(); + cell->set_block_id(block_structure.rows[i].cells[j].block_id); + cell->set_position(block_structure.rows[i].cells[j].position); + } + } +} +#endif + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_structure.h b/extern/libmv/third_party/ceres/internal/ceres/block_structure.h new file mode 100644 index 00000000000..f509067d216 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_structure.h @@ -0,0 +1,105 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Block structure objects are used to carry information about the +// dense block structure of sparse matrices. The BlockSparseMatrix +// object uses the BlockStructure objects to keep track of the matrix +// structure and operate upon it. This allows us to use more cache +// friendly block oriented linear algebra operations on the matrix +// instead of accessing it one scalar entry at a time. + +#ifndef CERES_INTERNAL_BLOCK_STRUCTURE_H_ +#define CERES_INTERNAL_BLOCK_STRUCTURE_H_ + +#include <vector> +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockStructureProto; + +typedef int16 BlockSize; + +struct Block { + Block() : size(-1), position(-1) {} + Block(int size_, int position_) : size(size_), position(position_) {} + + BlockSize size; + int position; // Position along the row/column. +}; + +struct Cell { + Cell() : block_id(-1), position(-1) {} + Cell(int block_id_, int position_) + : block_id(block_id_), position(position_) {} + + // Column or row block id as the case maybe. + int block_id; + // Where in the values array of the jacobian is this cell located. + int position; +}; + +// Order cell by their block_id; +bool CellLessThan(const Cell& lhs, const Cell& rhs); + +struct CompressedList { + Block block; + vector<Cell> cells; +}; + +typedef CompressedList CompressedRow; +typedef CompressedList CompressedColumn; + +struct CompressedRowBlockStructure { + vector<Block> cols; + vector<CompressedRow> rows; +}; + +struct CompressedColumnBlockStructure { + vector<Block> rows; + vector<CompressedColumn> cols; +}; + +// Deserialize the given block structure proto to the given block structure. +// Destroys previous contents of block_structure. +void ProtoToBlockStructure(const BlockStructureProto &proto, + CompressedRowBlockStructure *block_structure); + +// Serialize the given block structure to the given proto. Destroys previous +// contents of proto. +void BlockStructureToProto(const CompressedRowBlockStructure &block_structure, + BlockStructureProto *proto); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_STRUCTURE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc new file mode 100644 index 00000000000..53190ada6fc --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc @@ -0,0 +1,238 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: David Gallup (dgallup@google.com) +// Sameer Agarwal (sameeragarwal@google.com) + +#include "ceres/canonical_views_clustering.h" + +#include <glog/logging.h> +#include "ceres/graph.h" +#include "ceres/collections_port.h" +#include "ceres/map_util.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +typedef HashMap<int, int> IntMap; +typedef HashSet<int> IntSet; + +class CanonicalViewsClustering { + public: + CanonicalViewsClustering() {} + + // Compute the canonical views clustering of the vertices of the + // graph. centers will contain the vertices that are the identified + // as the canonical views/cluster centers, and membership is a map + // from vertices to cluster_ids. The i^th cluster center corresponds + // to the i^th cluster. It is possible depending on the + // configuration of the clustering algorithm that some of the + // vertices may not be assigned to any cluster. In this case they + // are assigned to a cluster with id = kInvalidClusterId. + void ComputeClustering(const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + IntMap* membership); + + private: + void FindValidViews(IntSet* valid_views) const; + double ComputeClusteringQualityDifference(const int candidate, + const vector<int>& centers) const; + void UpdateCanonicalViewAssignments(const int canonical_view); + void ComputeClusterMembership(const vector<int>& centers, + IntMap* membership) const; + + CanonicalViewsClusteringOptions options_; + const Graph<int>* graph_; + // Maps a view to its representative canonical view (its cluster + // center). + IntMap view_to_canonical_view_; + // Maps a view to its similarity to its current cluster center. + HashMap<int, double> view_to_canonical_view_similarity_; + DISALLOW_COPY_AND_ASSIGN(CanonicalViewsClustering); +}; + +void ComputeCanonicalViewsClustering( + const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + IntMap* membership) { + time_t start_time = time(NULL); + CanonicalViewsClustering cv; + cv.ComputeClustering(graph, options, centers, membership); + VLOG(2) << "Canonical views clustering time (secs): " + << time(NULL) - start_time; +} + +// Implementation of CanonicalViewsClustering +void CanonicalViewsClustering::ComputeClustering( + const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + IntMap* membership) { + options_ = options; + CHECK_NOTNULL(centers)->clear(); + CHECK_NOTNULL(membership)->clear(); + graph_ = &graph; + + IntSet valid_views; + FindValidViews(&valid_views); + while (valid_views.size() > 0) { + // Find the next best canonical view. + double best_difference = -std::numeric_limits<double>::max(); + int best_view = 0; + + // TODO(sameeragarwal): Make this loop multi-threaded. + for (IntSet::const_iterator view = valid_views.begin(); + view != valid_views.end(); + ++view) { + const double difference = + ComputeClusteringQualityDifference(*view, *centers); + if (difference > best_difference) { + best_difference = difference; + best_view = *view; + } + } + + CHECK_GT(best_difference, -std::numeric_limits<double>::max()); + + // Add canonical view if quality improves, or if minimum is not + // yet met, otherwise break. + if ((best_difference <= 0) && + (centers->size() >= options_.min_views)) { + break; + } + + centers->push_back(best_view); + valid_views.erase(best_view); + UpdateCanonicalViewAssignments(best_view); + } + + ComputeClusterMembership(*centers, membership); +} + +// Return the set of vertices of the graph which have valid vertex +// weights. +void CanonicalViewsClustering::FindValidViews( + IntSet* valid_views) const { + const IntSet& views = graph_->vertices(); + for (IntSet::const_iterator view = views.begin(); + view != views.end(); + ++view) { + if (graph_->VertexWeight(*view) != Graph<int>::InvalidWeight()) { + valid_views->insert(*view); + } + } +} + +// Computes the difference in the quality score if 'candidate' were +// added to the set of canonical views. +double CanonicalViewsClustering::ComputeClusteringQualityDifference( + const int candidate, + const vector<int>& centers) const { + // View score. + double difference = + options_.view_score_weight * graph_->VertexWeight(candidate); + + // Compute how much the quality score changes if the candidate view + // was added to the list of canonical views and its nearest + // neighbors became members of its cluster. + const IntSet& neighbors = graph_->Neighbors(candidate); + for (IntSet::const_iterator neighbor = neighbors.begin(); + neighbor != neighbors.end(); + ++neighbor) { + const double old_similarity = + FindWithDefault(view_to_canonical_view_similarity_, *neighbor, 0.0); + const double new_similarity = graph_->EdgeWeight(*neighbor, candidate); + if (new_similarity > old_similarity) { + difference += new_similarity - old_similarity; + } + } + + // Number of views penalty. + difference -= options_.size_penalty_weight; + + // Orthogonality. + for (int i = 0; i < centers.size(); ++i) { + difference -= options_.similarity_penalty_weight * + graph_->EdgeWeight(centers[i], candidate); + } + + return difference; +} + +// Reassign views if they're more similar to the new canonical view. +void CanonicalViewsClustering::UpdateCanonicalViewAssignments( + const int canonical_view) { + const IntSet& neighbors = graph_->Neighbors(canonical_view); + for (IntSet::const_iterator neighbor = neighbors.begin(); + neighbor != neighbors.end(); + ++neighbor) { + const double old_similarity = + FindWithDefault(view_to_canonical_view_similarity_, *neighbor, 0.0); + const double new_similarity = + graph_->EdgeWeight(*neighbor, canonical_view); + if (new_similarity > old_similarity) { + view_to_canonical_view_[*neighbor] = canonical_view; + view_to_canonical_view_similarity_[*neighbor] = new_similarity; + } + } +} + +// Assign a cluster id to each view. +void CanonicalViewsClustering::ComputeClusterMembership( + const vector<int>& centers, + IntMap* membership) const { + CHECK_NOTNULL(membership)->clear(); + + // The i^th cluster has cluster id i. + IntMap center_to_cluster_id; + for (int i = 0; i < centers.size(); ++i) { + center_to_cluster_id[centers[i]] = i; + } + + static const int kInvalidClusterId = -1; + + const IntSet& views = graph_->vertices(); + for (IntSet::const_iterator view = views.begin(); + view != views.end(); + ++view) { + IntMap::const_iterator it = + view_to_canonical_view_.find(*view); + int cluster_id = kInvalidClusterId; + if (it != view_to_canonical_view_.end()) { + cluster_id = FindOrDie(center_to_cluster_id, it->second); + } + + InsertOrDie(membership, *view, cluster_id); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h new file mode 100644 index 00000000000..2d1eb403995 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h @@ -0,0 +1,133 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// An implementation of the Canonical Views clustering algorithm from +// "Scene Summarization for Online Image Collections", Ian Simon, Noah +// Snavely, Steven M. Seitz, ICCV 2007. +// +// More details can be found at +// http://grail.cs.washington.edu/projects/canonview/ +// +// Ceres uses this algorithm to perform view clustering for +// constructing visibility based preconditioners. + +#ifndef CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ +#define CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ + +#include <vector> + +#include <glog/logging.h> +#include "ceres/collections_port.h" +#include "ceres/graph.h" +#include "ceres/map_util.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class CanonicalViewsClusteringOptions; + +// Compute a partitioning of the vertices of the graph using the +// canonical views clustering algorithm. +// +// In the following we will use the terms vertices and views +// interchangably. Given a weighted Graph G(V,E), the canonical views +// of G are the the set of vertices that best "summarize" the content +// of the graph. If w_ij i s the weight connecting the vertex i to +// vertex j, and C is the set of canonical views. Then the objective +// of the canonical views algorithm is +// +// E[C] = sum_[i in V] max_[j in C] w_ij +// - size_penalty_weight * |C| +// - similarity_penalty_weight * sum_[i in C, j in C, j > i] w_ij +// +// alpha is the size penalty that penalizes large number of canonical +// views. +// +// beta is the similarity penalty that penalizes canonical views that +// are too similar to other canonical views. +// +// Thus the canonical views algorithm tries to find a canonical view +// for each vertex in the graph which best explains it, while trying +// to minimize the number of canonical views and the overlap between +// them. +// +// We further augment the above objective function by allowing for per +// vertex weights, higher weights indicating a higher preference for +// being chosen as a canonical view. Thus if w_i is the vertex weight +// for vertex i, the objective function is then +// +// E[C] = sum_[i in V] max_[j in C] w_ij +// - size_penalty_weight * |C| +// - similarity_penalty_weight * sum_[i in C, j in C, j > i] w_ij +// + view_score_weight * sum_[i in C] w_i +// +// centers will contain the vertices that are the identified +// as the canonical views/cluster centers, and membership is a map +// from vertices to cluster_ids. The i^th cluster center corresponds +// to the i^th cluster. +// +// It is possible depending on the configuration of the clustering +// algorithm that some of the vertices may not be assigned to any +// cluster. In this case they are assigned to a cluster with id = -1; +void ComputeCanonicalViewsClustering( + const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + HashMap<int, int>* membership); + +struct CanonicalViewsClusteringOptions { + CanonicalViewsClusteringOptions() + : min_views(3), + size_penalty_weight(5.75), + similarity_penalty_weight(100.0), + view_score_weight(0.0) { + } + // The minimum number of canonical views to compute. + int min_views; + + // Penalty weight for the number of canonical views. A higher + // number will result in fewer canonical views. + double size_penalty_weight; + + // Penalty weight for the diversity (orthogonality) of the + // canonical views. A higher number will encourage less similar + // canonical views. + double similarity_penalty_weight; + + // Weight for per-view scores. Lower weight places less + // confidence in the view scores. + double view_score_weight; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/casts.h b/extern/libmv/third_party/ceres/internal/ceres/casts.h new file mode 100644 index 00000000000..99cf2186cc7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/casts.h @@ -0,0 +1,108 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CASTS_H_ +#define CERES_INTERNAL_CASTS_H_ + +#include <cassert> +#include <cstddef> // For NULL. + +namespace ceres { + +// Identity metafunction. +template <class T> +struct identity_ { + typedef T type; +}; + +// Use implicit_cast as a safe version of static_cast or const_cast +// for implicit conversions. For example: +// - Upcasting in a type hierarchy. +// - Performing arithmetic conversions (int32 to int64, int to double, etc.). +// - Adding const or volatile qualifiers. +// +// In general, implicit_cast can be used to convert this code +// To to = from; +// DoSomething(to); +// to this +// DoSomething(implicit_cast<To>(from)); +// +// base::identity_ is used to make a non-deduced context, which +// forces all callers to explicitly specify the template argument. +template<typename To> +inline To implicit_cast(typename identity_<To>::type to) { + return to; +} + +// This version of implicit_cast is used when two template arguments +// are specified. It's obsolete and should not be used. +template<typename To, typename From> +inline To implicit_cast(typename identity_<From>::type const &f) { + return f; +} + +// When you upcast (that is, cast a pointer from type Foo to type +// SuperclassOfFoo), it's fine to use implicit_cast<>, since upcasts +// always succeed. When you downcast (that is, cast a pointer from +// type Foo to type SubclassOfFoo), static_cast<> isn't safe, because +// how do you know the pointer is really of type SubclassOfFoo? It +// could be a bare Foo, or of type DifferentSubclassOfFoo. Thus, +// when you downcast, you should use this macro. In debug mode, we +// use dynamic_cast<> to double-check the downcast is legal (we die +// if it's not). In normal mode, we do the efficient static_cast<> +// instead. Thus, it's important to test in debug mode to make sure +// the cast is legal! +// This is the only place in the code we should use dynamic_cast<>. +// In particular, you SHOULDN'T be using dynamic_cast<> in order to +// do RTTI (eg code like this: +// if (dynamic_cast<Subclass1>(foo)) HandleASubclass1Object(foo); +// if (dynamic_cast<Subclass2>(foo)) HandleASubclass2Object(foo); +// You should design the code some other way not to need this. + +template<typename To, typename From> // use like this: down_cast<T*>(foo); +inline To down_cast(From* f) { // so we only accept pointers + // Ensures that To is a sub-type of From *. This test is here only + // for compile-time type checking, and has no overhead in an + // optimized build at run-time, as it will be optimized away + // completely. + + // TODO(csilvers): This should use COMPILE_ASSERT. + if (false) { + implicit_cast<From*, To>(NULL); + } + + // uses RTTI in dbg and fastbuild. asserts are disabled in opt builds. + assert(f == NULL || dynamic_cast<To>(f) != NULL); // NOLINT + return static_cast<To>(f); +} + +} // namespace ceres + +#endif // CERES_INTERNAL_CASTS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_linear_operator.h b/extern/libmv/third_party/ceres/internal/ceres/cgnr_linear_operator.h new file mode 100644 index 00000000000..f32d8d95c19 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_linear_operator.h @@ -0,0 +1,120 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ +#define CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ + +#include <algorithm> +#include "ceres/linear_operator.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +class SparseMatrix; + +// A linear operator which takes a matrix A and a diagonal vector D and +// performs products of the form +// +// (A^T A + D^T D)x +// +// This is used to implement iterative general sparse linear solving with +// conjugate gradients, where A is the Jacobian and D is a regularizing +// parameter. A brief proof that D^T D is the correct regularizer: +// +// Given a regularized least squares problem: +// +// min ||Ax - b||^2 + ||Dx||^2 +// x +// +// First expand into matrix notation: +// +// (Ax - b)^T (Ax - b) + xD^TDx +// +// Then multiply out to get: +// +// = xA^TAx - 2b^T Ax + b^Tb + xD^TDx +// +// Take the derivative: +// +// 0 = 2A^TAx - 2A^T b + 2 D^TDx +// 0 = A^TAx - A^T b + D^TDx +// 0 = (A^TA + D^TD)x - A^T b +// +// Thus, the symmetric system we need to solve for CGNR is +// +// Sx = z +// +// with S = A^TA + D^TD +// and z = A^T b +// +// Note: This class is not thread safe, since it uses some temporary storage. +class CgnrLinearOperator : public LinearOperator { + public: + CgnrLinearOperator(const LinearOperator& A, const double *D) + : A_(A), D_(D), z_(new double[A.num_rows()]) { + } + virtual ~CgnrLinearOperator() {} + + virtual void RightMultiply(const double* x, double* y) const { + std::fill(z_.get(), z_.get() + A_.num_rows(), 0.0); + + // z = Ax + A_.RightMultiply(x, z_.get()); + + // y = y + Atz + A_.LeftMultiply(z_.get(), y); + + // y = y + DtDx + if (D_ != NULL) { + int n = A_.num_cols(); + VectorRef(y, n).array() += ConstVectorRef(D_, n).array().square() * + ConstVectorRef(x, n).array(); + } + } + + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + + virtual int num_rows() const { return A_.num_cols(); } + virtual int num_cols() const { return A_.num_cols(); } + + private: + const LinearOperator& A_; + const double* D_; + scoped_array<double> z_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc new file mode 100644 index 00000000000..ccc8026f9f7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc @@ -0,0 +1,80 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/cgnr_solver.h" + +#include "glog/logging.h" +#include "ceres/linear_solver.h" +#include "ceres/cgnr_linear_operator.h" +#include "ceres/conjugate_gradients_solver.h" +#include "ceres/block_jacobi_preconditioner.h" + +namespace ceres { +namespace internal { + +CgnrSolver::CgnrSolver(const LinearSolver::Options& options) + : options_(options), + jacobi_preconditioner_(NULL) { +} + +LinearSolver::Summary CgnrSolver::Solve( + LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + // Form z = Atb. + scoped_array<double> z(new double[A->num_cols()]); + std::fill(z.get(), z.get() + A->num_cols(), 0.0); + A->LeftMultiply(b, z.get()); + + // Precondition if necessary. + LinearSolver::PerSolveOptions cg_per_solve_options = per_solve_options; + if (options_.preconditioner_type == JACOBI) { + if (jacobi_preconditioner_.get() == NULL) { + jacobi_preconditioner_.reset(new BlockJacobiPreconditioner(*A)); + } + jacobi_preconditioner_->Update(*A, per_solve_options.D); + cg_per_solve_options.preconditioner = jacobi_preconditioner_.get(); + } else if (options_.preconditioner_type != IDENTITY) { + LOG(FATAL) << "CGNR only supports IDENTITY and JACOBI preconditioners."; + } + + // Solve (AtA + DtD)x = z (= Atb). + std::fill(x, x + A->num_cols(), 0.0); + CgnrLinearOperator lhs(*A, per_solve_options.D); + ConjugateGradientsSolver conjugate_gradient_solver(options_); + return conjugate_gradient_solver.Solve(&lhs, + z.get(), + cg_per_solve_options, + x); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h new file mode 100644 index 00000000000..dd36f99006b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h @@ -0,0 +1,66 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CGNR_SOLVER_H_ +#define CERES_INTERNAL_CGNR_SOLVER_H_ + +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_solver.h" + +namespace ceres { +namespace internal { + +class BlockJacobiPreconditioner; + +// A conjugate gradients on the normal equations solver. This directly solves +// for the solution to +// +// (A^T A + D^T D)x = A^T b +// +// as required for solving for x in the least squares sense. Currently only +// block diagonal preconditioning is supported. +class CgnrSolver : public LinearSolver { + public: + explicit CgnrSolver(const LinearSolver::Options& options); + virtual Summary Solve(LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + private: + const LinearSolver::Options options_; + scoped_ptr<BlockJacobiPreconditioner> jacobi_preconditioner_; + DISALLOW_COPY_AND_ASSIGN(CgnrSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CGNR_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/collections_port.h b/extern/libmv/third_party/ceres/internal/ceres/collections_port.h new file mode 100644 index 00000000000..e125f3fffcd --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/collections_port.h @@ -0,0 +1,141 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Portable HashMap and HashSet, and a specialized overload for hashing pairs. + +#ifndef CERES_INTERNAL_COLLECTIONS_PORT_H_ +#define CERES_INTERNAL_COLLECTIONS_PORT_H_ + +#if defined(_MSC_VER) && _MSC_VER <= 1600 +#include <unordered_map> +#include <unordered_set> +#else +#include <tr1/unordered_map> +#include <tr1/unordered_set> +#endif +#include <utility> +#include "ceres/integral_types.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +template<typename K, typename V> +struct HashMap : tr1::unordered_map<K, V> {}; + +template<typename K> +struct HashSet : tr1::unordered_set<K> {}; + +#ifdef _WIN32 +#define GG_LONGLONG(x) x##I64 +#define GG_ULONGLONG(x) x##UI64 +#else +#define GG_LONGLONG(x) x##LL +#define GG_ULONGLONG(x) x##ULL +#endif + +// The hash function is due to Bob Jenkins (see +// http://burtleburtle.net/bob/hash/index.html). Each mix takes 36 instructions, +// in 18 cycles if you're lucky. On x86 architectures, this requires 45 +// instructions in 27 cycles, if you're lucky. +// +// 32bit version +inline void hash_mix(uint32& a, uint32& b, uint32& c) { + a -= b; a -= c; a ^= (c>>13); + b -= c; b -= a; b ^= (a<<8); + c -= a; c -= b; c ^= (b>>13); + a -= b; a -= c; a ^= (c>>12); + b -= c; b -= a; b ^= (a<<16); + c -= a; c -= b; c ^= (b>>5); + a -= b; a -= c; a ^= (c>>3); + b -= c; b -= a; b ^= (a<<10); + c -= a; c -= b; c ^= (b>>15); +} + +// 64bit version +inline void hash_mix(uint64& a, uint64& b, uint64& c) { + a -= b; a -= c; a ^= (c>>43); + b -= c; b -= a; b ^= (a<<9); + c -= a; c -= b; c ^= (b>>8); + a -= b; a -= c; a ^= (c>>38); + b -= c; b -= a; b ^= (a<<23); + c -= a; c -= b; c ^= (b>>5); + a -= b; a -= c; a ^= (c>>35); + b -= c; b -= a; b ^= (a<<49); + c -= a; c -= b; c ^= (b>>11); +} + +inline uint32 Hash32NumWithSeed(uint32 num, uint32 c) { + // The golden ratio; an arbitrary value. + uint32 b = 0x9e3779b9UL; + hash_mix(num, b, c); + return c; +} + +inline uint64 Hash64NumWithSeed(uint64 num, uint64 c) { + // More of the golden ratio. + uint64 b = GG_ULONGLONG(0xe08c1d668b756f82); + hash_mix(num, b, c); + return c; +} + +} // namespace internal +} // namespace ceres + +// Since on some platforms this is a doubly-nested namespace (std::tr1) and +// others it is not, the entire namespace line must be in a macro. +CERES_HASH_NAMESPACE_START + +// The outrageously annoying specializations below are for portability reasons. +// In short, it's not possible to have two overloads of hash<pair<T1, T2> + +// Hasher for STL pairs. Requires hashers for both members to be defined. +template<typename T> +struct hash<pair<T, T> > { + size_t operator()(const pair<T, T>& p) const { + size_t h1 = hash<T>()(p.first); + size_t h2 = hash<T>()(p.second); + // The decision below is at compile time + return (sizeof(h1) <= sizeof(ceres::internal::uint32)) ? + ceres::internal::Hash32NumWithSeed(h1, h2) : + ceres::internal::Hash64NumWithSeed(h1, h2); + } + // Less than operator for MSVC. + bool operator()(const pair<T, T>& a, + const pair<T, T>& b) const { + return a < b; + } + static const size_t bucket_size = 4; // These are required by MSVC + static const size_t min_buckets = 8; // 4 and 8 are defaults. +}; + +CERES_HASH_NAMESPACE_END + +#endif // CERES_INTERNAL_COLLECTIONS_PORT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.cc b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.cc new file mode 100644 index 00000000000..aa883b7d353 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.cc @@ -0,0 +1,201 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/compressed_row_jacobian_writer.h" + +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/scratch_evaluate_preparer.h" + +namespace ceres { +namespace internal { + +SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { + const vector<ResidualBlock*>& residual_blocks = + program_->residual_blocks(); + + int total_num_residuals = program_->NumResiduals(); + int total_num_effective_parameters = program_->NumEffectiveParameters(); + + // Count the number of jacobian nonzeros. + int num_jacobian_nonzeros = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + num_jacobian_nonzeros += num_residuals * parameter_block->LocalSize(); + } + } + } + + // Allocate storage for the jacobian with some extra space at the end. + // Allocate more space than needed to store the jacobian so that when the LM + // algorithm adds the diagonal, no reallocation is necessary. This reduces + // peak memory usage significantly. + CompressedRowSparseMatrix* jacobian = + new CompressedRowSparseMatrix( + total_num_residuals, + total_num_effective_parameters, + num_jacobian_nonzeros + total_num_effective_parameters); + + // At this stage, the CompressedSparseMatrix is an invalid state. But this + // seems to be the only way to construct it without doing a memory copy. + int* rows = jacobian->mutable_rows(); + int* cols = jacobian->mutable_cols(); + int row_pos = 0; + rows[0] = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Count the number of derivatives for a row of this residual block and + // build a list of active parameter block indices. + int num_derivatives = 0; + vector<int> parameter_indices; + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + parameter_indices.push_back(parameter_block->index()); + num_derivatives += parameter_block->LocalSize(); + } + } + + // Sort the parameters by their position in the state vector. + sort(parameter_indices.begin(), parameter_indices.end()); + CHECK(unique(parameter_indices.begin(), parameter_indices.end()) == + parameter_indices.end()) + << "Ceres internal error: " + << "Duplicate parameter blocks detected in a cost function. " + << "This should never happen. Please report this to " + << "the Ceres developers."; + + // Update the row indices. + const int num_residuals = residual_block->NumResiduals(); + for (int j = 0; j < num_residuals; ++j) { + rows[row_pos + j + 1] = rows[row_pos + j] + num_derivatives; + } + + // Iterate over parameter blocks in the order which they occur in the + // parameter vector. This code mirrors that in Write(), where jacobian + // values are updated. + int col_pos = 0; + for (int j = 0; j < parameter_indices.size(); ++j) { + ParameterBlock* parameter_block = + program_->parameter_blocks()[parameter_indices[j]]; + const int parameter_block_size = parameter_block->LocalSize(); + + for (int r = 0; r < num_residuals; ++r) { + // This is the position in the values array of the jacobian where this + // row of the jacobian block should go. + const int column_block_begin = rows[row_pos + r] + col_pos; + + for (int c = 0; c < parameter_block_size; ++c) { + cols[column_block_begin + c] = parameter_block->delta_offset() + c; + } + } + col_pos += parameter_block_size; + } + row_pos += num_residuals; + } + + CHECK_EQ(num_jacobian_nonzeros, rows[total_num_residuals]); + return jacobian; +} + +void CompressedRowJacobianWriter::Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian) { + CompressedRowSparseMatrix* jacobian = + down_cast<CompressedRowSparseMatrix*>(base_jacobian); + + double* jacobian_values = jacobian->mutable_values(); + const int* jacobian_rows = jacobian->rows(); + + const ResidualBlock* residual_block = + program_->residual_blocks()[residual_id]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + const int num_residuals = residual_block->NumResiduals(); + + // It is necessary to determine the order of the jacobian blocks before + // copying them into the CompressedRowSparseMatrix. Just because a cost + // function uses parameter blocks 1 after 2 in its arguments does not mean + // that the block 1 occurs before block 2 in the column layout of the + // jacobian. Thus, determine the order by sorting the jacobian blocks by their + // position in the state vector. + vector<pair<int, int> > evaluated_jacobian_blocks; + for (int j = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + evaluated_jacobian_blocks.push_back( + make_pair(parameter_block->index(), j)); + } + } + sort(evaluated_jacobian_blocks.begin(), evaluated_jacobian_blocks.end()); + + // Where in the current row does the jacobian for a parameter block begin. + int col_pos = 0; + + // Iterate over the jacobian blocks in increasing order of their + // positions in the reduced parameter vector. + for (int i = 0; i < evaluated_jacobian_blocks.size(); ++i) { + const ParameterBlock* parameter_block = + program_->parameter_blocks()[evaluated_jacobian_blocks[i].first]; + const int argument = evaluated_jacobian_blocks[i].second; + const int parameter_block_size = parameter_block->LocalSize(); + + // Copy one row of the jacobian block at a time. + for (int r = 0; r < num_residuals; ++r) { + // Position of the r^th row of the current jacobian block. + const double* block_row_begin = + jacobians[argument] + r * parameter_block_size; + + // Position in the values array of the jacobian where this + // row of the jacobian block should go. + double* column_block_begin = + jacobian_values + jacobian_rows[residual_offset + r] + col_pos; + + copy(block_row_begin, + block_row_begin + parameter_block_size, + column_block_begin); + } + col_pos += parameter_block_size; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.h b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.h new file mode 100644 index 00000000000..c103165eaf1 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.h @@ -0,0 +1,75 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A jacobian writer that directly writes to compressed row sparse matrices. + +#ifndef CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ + +#include "ceres/evaluator.h" +#include "ceres/scratch_evaluate_preparer.h" + +namespace ceres { +namespace internal { + +class Program; +class SparseMatrix; + +class CompressedRowJacobianWriter { + public: + CompressedRowJacobianWriter(Evaluator::Options /* ignored */, + Program* program) + : program_(program) { + } + + // JacobianWriter interface. + + // Since the compressed row matrix has different layout than that assumed by + // the cost functions, use scratch space to store the jacobians temporarily + // then copy them over to the larger jacobian in the Write() function. + ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads) { + return ScratchEvaluatePreparer::Create(*program_, num_threads); + } + + SparseMatrix* CreateJacobian() const; + + void Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian); + + private: + Program* program_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc new file mode 100644 index 00000000000..95edf5396af --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc @@ -0,0 +1,334 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/compressed_row_sparse_matrix.h" + +#include <algorithm> +#include <vector> +#include "ceres/matrix_proto.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { +namespace { + +// Helper functor used by the constructor for reordering the contents +// of a TripletSparseMatrix. +struct RowColLessThan { + RowColLessThan(const int* rows, const int* cols) + : rows(rows), cols(cols) { + } + + bool operator()(const int x, const int y) const { + if (rows[x] == rows[y]) { + return (cols[x] < cols[y]); + } + return (rows[x] < rows[y]); + } + + const int* rows; + const int* cols; +}; + +} // namespace + +// This constructor gives you a semi-initialized CompressedRowSparseMatrix. +CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros) { + num_rows_ = num_rows; + num_cols_ = num_cols; + max_num_nonzeros_ = max_num_nonzeros; + + VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ + << " max_num_nonzeros: " << max_num_nonzeros_ + << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(double); // NOLINT + + rows_.reset(new int[num_rows_ + 1]); + cols_.reset(new int[max_num_nonzeros_]); + values_.reset(new double[max_num_nonzeros_]); + + fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); + fill(cols_.get(), cols_.get() + max_num_nonzeros_, 0); + fill(values_.get(), values_.get() + max_num_nonzeros_, 0); +} + +CompressedRowSparseMatrix::CompressedRowSparseMatrix( + const TripletSparseMatrix& m) { + num_rows_ = m.num_rows(); + num_cols_ = m.num_cols(); + max_num_nonzeros_ = m.max_num_nonzeros(); + + // index is the list of indices into the TripletSparseMatrix m. + vector<int> index(m.num_nonzeros(), 0); + for (int i = 0; i < m.num_nonzeros(); ++i) { + index[i] = i; + } + + // Sort index such that the entries of m are ordered by row and ties + // are broken by column. + sort(index.begin(), index.end(), RowColLessThan(m.rows(), m.cols())); + + VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ + << " max_num_nonzeros: " << max_num_nonzeros_ + << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(double); // NOLINT + + rows_.reset(new int[num_rows_ + 1]); + cols_.reset(new int[max_num_nonzeros_]); + values_.reset(new double[max_num_nonzeros_]); + + // rows_ = 0 + fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); + + // Copy the contents of the cols and values array in the order given + // by index and count the number of entries in each row. + for (int i = 0; i < m.num_nonzeros(); ++i) { + const int idx = index[i]; + ++rows_[m.rows()[idx] + 1]; + cols_[i] = m.cols()[idx]; + values_[i] = m.values()[idx]; + } + + // Find the cumulative sum of the row counts. + for (int i = 1; i < num_rows_ + 1; ++i) { + rows_[i] += rows_[i-1]; + } + + CHECK_EQ(num_nonzeros(), m.num_nonzeros()); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +CompressedRowSparseMatrix::CompressedRowSparseMatrix( + const SparseMatrixProto& outer_proto) { + CHECK(outer_proto.has_compressed_row_matrix()); + + const CompressedRowSparseMatrixProto& proto = + outer_proto.compressed_row_matrix(); + + num_rows_ = proto.num_rows(); + num_cols_ = proto.num_cols(); + + rows_.reset(new int[proto.rows_size()]); + cols_.reset(new int[proto.cols_size()]); + values_.reset(new double[proto.values_size()]); + + for (int i = 0; i < proto.rows_size(); ++i) { + rows_[i] = proto.rows(i); + } + + CHECK_EQ(proto.rows_size(), num_rows_ + 1); + CHECK_EQ(proto.cols_size(), proto.values_size()); + CHECK_EQ(proto.cols_size(), rows_[num_rows_]); + + for (int i = 0; i < proto.cols_size(); ++i) { + cols_[i] = proto.cols(i); + values_[i] = proto.values(i); + } + + max_num_nonzeros_ = proto.cols_size(); +} +#endif + +CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, + int num_rows) { + CHECK_NOTNULL(diagonal); + + num_rows_ = num_rows; + num_cols_ = num_rows; + max_num_nonzeros_ = num_rows; + + rows_.reset(new int[num_rows_ + 1]); + cols_.reset(new int[num_rows_]); + values_.reset(new double[num_rows_]); + + rows_[0] = 0; + for (int i = 0; i < num_rows_; ++i) { + cols_[i] = i; + values_[i] = diagonal[i]; + rows_[i + 1] = i + 1; + } + + CHECK_EQ(num_nonzeros(), num_rows); +} + +CompressedRowSparseMatrix::~CompressedRowSparseMatrix() { +} + +void CompressedRowSparseMatrix::SetZero() { + fill(values_.get(), values_.get() + num_nonzeros(), 0.0); +} + +void CompressedRowSparseMatrix::RightMultiply(const double* x, + double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + y[r] += values_[idx] * x[cols_[idx]]; + } + } +} + +void CompressedRowSparseMatrix::LeftMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + y[cols_[idx]] += values_[idx] * x[r]; + } + } +} + +void CompressedRowSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + + fill(x, x + num_cols_, 0.0); + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + x[cols_[idx]] += values_[idx] * values_[idx]; + } +} + +void CompressedRowSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + values_[idx] *= scale[cols_[idx]]; + } +} + +void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + CHECK_NOTNULL(dense_matrix); + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + (*dense_matrix)(r, cols_[idx]) = values_[idx]; + } + } +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void CompressedRowSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { + CHECK_NOTNULL(outer_proto); + + outer_proto->Clear(); + CompressedRowSparseMatrixProto* proto + = outer_proto->mutable_compressed_row_matrix(); + + proto->set_num_rows(num_rows_); + proto->set_num_cols(num_cols_); + + for (int r = 0; r < num_rows_ + 1; ++r) { + proto->add_rows(rows_[r]); + } + + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + proto->add_cols(cols_[idx]); + proto->add_values(values_[idx]); + } +} +#endif + +void CompressedRowSparseMatrix::DeleteRows(int delta_rows) { + CHECK_GE(delta_rows, 0); + CHECK_LE(delta_rows, num_rows_); + + int new_num_rows = num_rows_ - delta_rows; + + num_rows_ = new_num_rows; + int* new_rows = new int[num_rows_ + 1]; + copy(rows_.get(), rows_.get() + num_rows_ + 1, new_rows); + rows_.reset(new_rows); +} + +void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { + CHECK_EQ(m.num_cols(), num_cols_); + + // Check if there is enough space. If not, then allocate new arrays + // to hold the combined matrix and copy the contents of this matrix + // into it. + if (max_num_nonzeros_ < num_nonzeros() + m.num_nonzeros()) { + int new_max_num_nonzeros = num_nonzeros() + m.num_nonzeros(); + + VLOG(1) << "Reallocating " << sizeof(int) * new_max_num_nonzeros; // NOLINT + + int* new_cols = new int[new_max_num_nonzeros]; + copy(cols_.get(), cols_.get() + max_num_nonzeros_, new_cols); + cols_.reset(new_cols); + + double* new_values = new double[new_max_num_nonzeros]; + copy(values_.get(), values_.get() + max_num_nonzeros_, new_values); + values_.reset(new_values); + + max_num_nonzeros_ = new_max_num_nonzeros; + } + + // Copy the contents of m into this matrix. + copy(m.cols(), m.cols() + m.num_nonzeros(), cols_.get() + num_nonzeros()); + copy(m.values(), + m.values() + m.num_nonzeros(), + values_.get() + num_nonzeros()); + + // Create the new rows array to hold the enlarged matrix. + int* new_rows = new int[num_rows_ + m.num_rows() + 1]; + // The first num_rows_ entries are the same + copy(rows_.get(), rows_.get() + num_rows_, new_rows); + + // new_rows = [rows_, m.row() + rows_[num_rows_]] + fill(new_rows + num_rows_, + new_rows + num_rows_ + m.num_rows() + 1, + rows_[num_rows_]); + + for (int r = 0; r < m.num_rows() + 1; ++r) { + new_rows[num_rows_ + r] += m.rows()[r]; + } + + rows_.reset(new_rows); + num_rows_ += m.num_rows(); +} + +void CompressedRowSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + fprintf(file, "% 10d % 10d %17f\n", r, cols_[idx], values_[idx]); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h new file mode 100644 index 00000000000..9a39d28e111 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ + +#include <glog/logging.h> +#include "ceres/sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; + +class CompressedRowSparseMatrix : public SparseMatrix { + public: + // Build a matrix with the same content as the TripletSparseMatrix + // m. TripletSparseMatrix objects are easier to construct + // incrementally, so we use them to initialize SparseMatrix + // objects. + // + // We assume that m does not have any repeated entries. + explicit CompressedRowSparseMatrix(const TripletSparseMatrix& m); +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit CompressedRowSparseMatrix(const SparseMatrixProto& proto); +#endif + + // Use this constructor only if you know what you are doing. This + // creates a "blank" matrix with the appropriate amount of memory + // allocated. However, the object itself is in an inconsistent state + // as the rows and cols matrices do not match the values of + // num_rows, num_cols and max_num_nonzeros. + // + // The use case for this constructor is that when the user knows the + // size of the matrix to begin with and wants to update the layout + // manually, instead of going via the indirect route of first + // constructing a TripletSparseMatrix, which leads to more than + // double the peak memory usage. + CompressedRowSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros); + + // Build a square sparse diagonal matrix with num_rows rows and + // columns. The diagonal m(i,i) = diagonal(i); + CompressedRowSparseMatrix(const double* diagonal, int num_rows); + + virtual ~CompressedRowSparseMatrix(); + + // SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto* proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_cols_; } + virtual int num_nonzeros() const { return rows_[num_rows_]; } + virtual const double* values() const { return values_.get(); } + virtual double* mutable_values() { return values_.get(); } + + // Delete the bottom delta_rows. + // num_rows -= delta_rows + void DeleteRows(int delta_rows); + + // Append the contents of m to the bottom of this matrix. m must + // have the same number of columns as this matrix. + void AppendRows(const CompressedRowSparseMatrix& m); + + // Low level access methods that expose the structure of the matrix. + const int* cols() const { return cols_.get(); } + int* mutable_cols() { return cols_.get(); } + + const int* rows() const { return rows_.get(); } + int* mutable_rows() { return rows_.get(); } + + private: + scoped_array<int> cols_; + scoped_array<int> rows_; + scoped_array<double> values_; + + int num_rows_; + int num_cols_; + + int max_num_nonzeros_; + + DISALLOW_COPY_AND_ASSIGN(CompressedRowSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/conditioned_cost_function.cc b/extern/libmv/third_party/ceres/internal/ceres/conditioned_cost_function.cc new file mode 100644 index 00000000000..ca80bfb9c9d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/conditioned_cost_function.cc @@ -0,0 +1,130 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: wjr@google.com (William Rucklidge) +// +// This file contains the implementation of the conditioned cost function. + +#include "ceres/conditioned_cost_function.h" + +#include <cstddef> + +#include <glog/logging.h> +#include "ceres/stl_util.h" +#include "ceres/internal/eigen.h" +#include "ceres/types.h" + +namespace ceres { + +// This cost function has the same dimensions (parameters, residuals) as +// the one it's wrapping. +ConditionedCostFunction::ConditionedCostFunction( + CostFunction* wrapped_cost_function, + const vector<CostFunction*>& conditioners, + Ownership ownership) + : wrapped_cost_function_(wrapped_cost_function), + conditioners_(conditioners), + ownership_(ownership) { + // Set up our dimensions. + set_num_residuals(wrapped_cost_function_->num_residuals()); + *mutable_parameter_block_sizes() = + wrapped_cost_function_->parameter_block_sizes(); + + // Sanity-check the conditioners' dimensions. + CHECK_EQ(wrapped_cost_function_->num_residuals(), conditioners_.size()); + for (int i = 0; i < wrapped_cost_function_->num_residuals(); i++) { + if (conditioners[i]) { + CHECK_EQ(1, conditioners[i]->num_residuals()); + CHECK_EQ(1, conditioners[i]->parameter_block_sizes().size()); + CHECK_EQ(1, conditioners[i]->parameter_block_sizes()[0]); + } + } +} + +ConditionedCostFunction::~ConditionedCostFunction() { + if (ownership_ == TAKE_OWNERSHIP) { + STLDeleteElements(&conditioners_); + } else { + wrapped_cost_function_.release(); + } +} + +bool ConditionedCostFunction::Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + bool success = wrapped_cost_function_->Evaluate(parameters, residuals, + jacobians); + if (!success) { + return false; + } + + for (int r = 0; r < wrapped_cost_function_->num_residuals(); r++) { + // On output, we want to have + // residuals[r] = conditioners[r](wrapped_residuals[r]) + // For parameter block i, column c, + // jacobians[i][r*parameter_block_size_[i] + c] = + // = d residual[r] / d parameters[i][c] + // = conditioners[r]'(wrapped_residuals[r]) * + // d wrapped_residuals[r] / d parameters[i][c] + if (conditioners_[r]) { + double conditioner_derivative; + double* conditioner_derivative_pointer = &conditioner_derivative; + double** conditioner_derivative_pointer2 = + &conditioner_derivative_pointer; + if (!jacobians) { + conditioner_derivative_pointer2 = NULL; + } + + double unconditioned_residual = residuals[r]; + double* parameter_pointer = &unconditioned_residual; + success = conditioners_[r]->Evaluate(¶meter_pointer, + &residuals[r], + conditioner_derivative_pointer2); + if (!success) { + return false; + } + + if (jacobians) { + for (int i = 0; + i < wrapped_cost_function_->parameter_block_sizes().size(); + i++) { + if (jacobians[i]) { + int parameter_block_size = + wrapped_cost_function_->parameter_block_sizes()[i]; + VectorRef jacobian_row(jacobians[i] + r * parameter_block_size, + parameter_block_size, 1); + jacobian_row *= conditioner_derivative; + } + } + } + } + } + return true; +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc new file mode 100644 index 00000000000..75f9e043fa5 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc @@ -0,0 +1,233 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A preconditioned conjugate gradients solver +// (ConjugateGradientsSolver) for positive semidefinite linear +// systems. +// +// We have also augmented the termination criterion used by this +// solver to support not just residual based termination but also +// termination based on decrease in the value of the quadratic model +// that CG optimizes. + +#include "ceres/conjugate_gradients_solver.h" + +#include <cmath> +#include <cstddef> +#include <glog/logging.h> +#include "ceres/linear_operator.h" +#include "ceres/internal/eigen.h" +#include "ceres/types.h" +#include "ceres/jet.h" + +namespace ceres { +namespace internal { +namespace { + +bool IsZeroOrInfinity(double x) { + return ((x == 0.0) || (isinf(x))); +} + +// Constant used in the MATLAB implementation ~ 2 * eps. +const double kEpsilon = 2.2204e-16; + +} // namespace + +ConjugateGradientsSolver::ConjugateGradientsSolver( + const LinearSolver::Options& options) + : options_(options) { +} + +LinearSolver::Summary ConjugateGradientsSolver::Solve( + LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(x); + CHECK_NOTNULL(b); + CHECK_EQ(A->num_rows(), A->num_cols()); + + LinearSolver::Summary summary; + summary.termination_type = MAX_ITERATIONS; + summary.num_iterations = 0; + + int num_cols = A->num_cols(); + VectorRef xref(x, num_cols); + ConstVectorRef bref(b, num_cols); + + double norm_b = bref.norm(); + if (norm_b == 0.0) { + xref.setZero(); + summary.termination_type = TOLERANCE; + return summary; + } + + Vector r(num_cols); + Vector p(num_cols); + Vector z(num_cols); + Vector tmp(num_cols); + + double tol_r = per_solve_options.r_tolerance * norm_b; + + tmp.setZero(); + A->RightMultiply(x, tmp.data()); + r = bref - tmp; + double norm_r = r.norm(); + + if (norm_r <= tol_r) { + summary.termination_type = TOLERANCE; + return summary; + } + + double rho = 1.0; + + // Initial value of the quadratic model Q = x'Ax - 2 * b'x. + double Q0 = -1.0 * xref.dot(bref + r); + + for (summary.num_iterations = 1; + summary.num_iterations < options_.max_num_iterations; + ++summary.num_iterations) { + VLOG(2) << "cg iteration " << summary.num_iterations; + + // Apply preconditioner + if (per_solve_options.preconditioner != NULL) { + z.setZero(); + per_solve_options.preconditioner->RightMultiply(r.data(), z.data()); + } else { + z = r; + } + + double last_rho = rho; + rho = r.dot(z); + + if (IsZeroOrInfinity(rho)) { + LOG(ERROR) << "Numerical failure. rho = " << rho; + summary.termination_type = FAILURE; + break; + }; + + if (summary.num_iterations == 1) { + p = z; + } else { + double beta = rho / last_rho; + if (IsZeroOrInfinity(beta)) { + LOG(ERROR) << "Numerical failure. beta = " << beta; + summary.termination_type = FAILURE; + break; + } + p = z + beta * p; + } + + Vector& q = z; + q.setZero(); + A->RightMultiply(p.data(), q.data()); + double pq = p.dot(q); + + if ((pq <= 0) || isinf(pq)) { + LOG(ERROR) << "Numerical failure. pq = " << pq; + summary.termination_type = FAILURE; + break; + } + + double alpha = rho / pq; + if (isinf(alpha)) { + LOG(ERROR) << "Numerical failure. alpha " << alpha; + summary.termination_type = FAILURE; + break; + } + + xref = xref + alpha * p; + + // Ideally we would just use the update r = r - alpha*q to keep + // track of the residual vector. However this estimate tends to + // drift over time due to round off errors. Thus every + // residual_reset_period iterations, we calculate the residual as + // r = b - Ax. We do not do this every iteration because this + // requires an additional matrix vector multiply which would + // double the complexity of the CG algorithm. + if (summary.num_iterations % options_.residual_reset_period == 0) { + tmp.setZero(); + A->RightMultiply(x, tmp.data()); + r = bref - tmp; + } else { + r = r - alpha * q; + } + + // Quadratic model based termination. + // Q1 = x'Ax - 2 * b' x. + double Q1 = -1.0 * xref.dot(bref + r); + + // For PSD matrices A, let + // + // Q(x) = x'Ax - 2b'x + // + // be the cost of the quadratic function defined by A and b. Then, + // the solver terminates at iteration i if + // + // i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance. + // + // This termination criterion is more useful when using CG to + // solve the Newton step. This particular convergence test comes + // from Stephen Nash's work on truncated Newton + // methods. References: + // + // 1. Stephen G. Nash & Ariela Sofer, Assessing A Search + // Direction Within A Truncated Newton Method, Operation + // Research Letters 9(1990) 219-221. + // + // 2. Stephen G. Nash, A Survey of Truncated Newton Methods, + // Journal of Computational and Applied Mathematics, + // 124(1-2), 45-59, 2000. + // + double zeta = summary.num_iterations * (Q1 - Q0) / Q1; + VLOG(2) << "Q termination: zeta " << zeta + << " " << per_solve_options.q_tolerance; + if (zeta < per_solve_options.q_tolerance) { + summary.termination_type = TOLERANCE; + break; + } + Q0 = Q1; + + // Residual based termination. + norm_r = r. norm(); + VLOG(2) << "R termination: norm_r " << norm_r + << " " << tol_r; + if (norm_r <= tol_r) { + summary.termination_type = TOLERANCE; + break; + } + } + + return summary; +}; + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.h b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.h new file mode 100644 index 00000000000..57f99e31db7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.h @@ -0,0 +1,74 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Preconditioned Conjugate Gradients based solver for positive +// semidefinite linear systems. + +#ifndef CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ +#define CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class LinearOperator; + +// This class implements the now classical Conjugate Gradients +// algorithm of Hestenes & Stiefel for solving postive semidefinite +// linear sytems. Optionally it can use a preconditioner also to +// reduce the condition number of the linear system and improve the +// convergence rate. Modern references for Conjugate Gradients are the +// books by Yousef Saad and Trefethen & Bau. This implementation of CG +// has been augmented with additional termination tests that are +// needed for forcing early termination when used as part of an +// inexact Newton solver. +// +// For more details see the documentation for +// LinearSolver::PerSolveOptions::r_tolerance and +// LinearSolver::PerSolveOptions::q_tolerance in linear_solver.h. +class ConjugateGradientsSolver : public LinearSolver { + public: + explicit ConjugateGradientsSolver(const LinearSolver::Options& options); + virtual Summary Solve(LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + private: + const LinearSolver::Options options_; + DISALLOW_COPY_AND_ASSIGN(ConjugateGradientsSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/corrector.cc b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc new file mode 100644 index 00000000000..4ca2c6f6c86 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc @@ -0,0 +1,125 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/corrector.h" + +#include <cstddef> +#include <cmath> +#include <glog/logging.h> +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +Corrector::Corrector(double sq_norm, const double rho[3]) { + CHECK_GE(sq_norm, 0.0); + CHECK_GT(rho[1], 0.0); + sqrt_rho1_ = sqrt(rho[1]); + + // If sq_norm = 0.0, the correction becomes trivial, the residual + // and the jacobian are scaled by the squareroot of the derivative + // of rho. Handling this case explicitly avoids the divide by zero + // error that would occur below. + // + // The case where rho'' < 0 also gets special handling. Technically + // it shouldn't, and the computation of the scaling should proceed + // as below, however we found in experiments that applying the + // curvature correction when rho'' < 0, which is the case when we + // are in the outlier region slows down the convergence of the + // algorithm significantly. + // + // Thus, we have divided the action of the robustifier into two + // parts. In the inliner region, we do the full second order + // correction which re-wights the gradient of the function by the + // square root of the derivative of rho, and the Gauss-Newton + // Hessian gets both the scaling and the rank-1 curvature + // correction. Normaly, alpha is upper bounded by one, but with this + // change, alpha is bounded above by zero. + // + // Empirically we have observed that the full Triggs correction and + // the clamped correction both start out as very good approximations + // to the loss function when we are in the convex part of the + // function, but as the function starts transitioning from convex to + // concave, the Triggs approximation diverges more and more and + // ultimately becomes linear. The clamped Triggs model however + // remains quadratic. + // + // The reason why the Triggs approximation becomes so poor is + // because the curvature correction that it applies to the gauss + // newton hessian goes from being a full rank correction to a rank + // deficient correction making the inversion of the Hessian fraught + // with all sorts of misery and suffering. + // + // The clamped correction retains its quadratic nature and inverting it + // is always well formed. + if ((sq_norm == 0.0) || (rho[2] <= 0.0)) { + residual_scaling_ = sqrt_rho1_; + alpha_sq_norm_ = 0.0; + return; + } + + // Calculate the smaller of the two solutions to the equation + // + // 0.5 * alpha^2 - alpha - rho'' / rho' * z'z = 0. + // + // Start by calculating the discriminant D. + const double D = 1.0 + 2.0 * sq_norm*rho[2] / rho[1]; + + // Since both rho[1] and rho[2] are guaranteed to be positive at + // this point, we know that D > 1.0. + + const double alpha = 1.0 - sqrt(D); + + // Calculate the constants needed by the correction routines. + residual_scaling_ = sqrt_rho1_ / (1 - alpha); + alpha_sq_norm_ = alpha / sq_norm; +} + +void Corrector::CorrectResiduals(int nrow, double* residuals) { + DCHECK(residuals != NULL); + VectorRef r_ref(residuals, nrow); + // Equation 11 in BANS. + r_ref *= residual_scaling_; +} + +void Corrector::CorrectJacobian(int nrow, int ncol, + double* residuals, double* jacobian) { + DCHECK(residuals != NULL); + DCHECK(jacobian != NULL); + ConstVectorRef r_ref(residuals, nrow); + MatrixRef j_ref(jacobian, nrow, ncol); + + // Equation 11 in BANS. + j_ref = sqrt_rho1_ * (j_ref - alpha_sq_norm_ * + r_ref * (r_ref.transpose() * j_ref)); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/corrector.h b/extern/libmv/third_party/ceres/internal/ceres/corrector.h new file mode 100644 index 00000000000..9914641cb01 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/corrector.h @@ -0,0 +1,88 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Class definition for the object that is responsible for applying a +// second order correction to the Gauss-Newton based on the ideas in +// BANS by Triggs et al. + +#ifndef CERES_INTERNAL_CORRECTOR_H_ +#define CERES_INTERNAL_CORRECTOR_H_ + +namespace ceres { +namespace internal { + +// Corrector is responsible for applying the second order correction +// to the residual and jacobian of a least squares problem based on a +// radial robust loss. +// +// The key idea here is to look at the expressions for the robustified +// gauss newton approximation and then take its squareroot to get the +// corresponding corrections to the residual and jacobian. For the +// full expressions see Eq. 10 and 11 in BANS by Triggs et al. +class Corrector { + public: + // The constructor takes the squared norm, the value, the first and + // second derivatives of the LossFunction. It precalculates some of + // the constants that are needed to apply the correction. The + // correction constant alpha is constrained to be smaller than 1, if + // it becomes larger than 1, then it will reverse the sign of the + // residual and the correction. If alpha is equal to 1 will result + // in a divide by zero error. Thus we constrain alpha to be upper + // bounded by 1 - epsilon_. + // + // rho[1] needs to be positive. The constructor will crash if this + // condition is not met. + // + // In practical use CorrectJacobian should always be called before + // CorrectResidual, because the jacobian correction depends on the + // value of the uncorrected residual values. + explicit Corrector(double sq_norm, const double rho[3]); + + // residuals *= sqrt(rho[1]) / (1 - alpha) + void CorrectResiduals(int nrow, double* residuals); + + // jacobian = sqrt(rho[1]) * jacobian - + // sqrt(rho[1]) * alpha / sq_norm * residuals residuals' * jacobian. + // + // The method assumes that the jacobian has row-major storage. It is + // the caller's responsibility to ensure that the pointer to + // jacobian is not null. + void CorrectJacobian(int nrow, int ncol, + double* residuals, double* jacobian); + + private: + double sqrt_rho1_; + double residual_scaling_; + double alpha_sq_norm_; +}; +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CORRECTOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_jacobian_writer.h b/extern/libmv/third_party/ceres/internal/ceres/dense_jacobian_writer.h new file mode 100644 index 00000000000..1177b83a556 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_jacobian_writer.h @@ -0,0 +1,110 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A jacobian writer that writes to dense Eigen matrices. + +#ifndef CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ + +#include "ceres/casts.h" +#include "ceres/dense_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/scratch_evaluate_preparer.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +class DenseJacobianWriter { + public: + DenseJacobianWriter(Evaluator::Options /* ignored */, + Program* program) + : program_(program) { + } + + // JacobianWriter interface. + + // Since the dense matrix has different layout than that assumed by the cost + // functions, use scratch space to store the jacobians temporarily then copy + // them over to the larger jacobian later. + ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads) { + return ScratchEvaluatePreparer::Create(*program_, num_threads); + } + + SparseMatrix* CreateJacobian() const { + return new DenseSparseMatrix(program_->NumResiduals(), + program_->NumEffectiveParameters()); + } + + void Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* jacobian) { + DenseSparseMatrix* dense_jacobian; + if (jacobian != NULL) { + dense_jacobian = down_cast<DenseSparseMatrix*>(jacobian); + } + const ResidualBlock* residual_block = + program_->residual_blocks()[residual_id]; + int num_parameter_blocks = residual_block->NumParameterBlocks(); + int num_residuals = residual_block->NumResiduals(); + + // Now copy the jacobians for each parameter into the dense jacobian matrix. + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + + // If the parameter block is fixed, then there is nothing to do. + if (parameter_block->IsConstant()) { + continue; + } + + int parameter_block_size = parameter_block->LocalSize(); + MatrixRef parameter_jacobian(jacobians[j], + num_residuals, + parameter_block_size); + + dense_jacobian->mutable_matrix().block( + residual_offset, + parameter_block->delta_offset(), + num_residuals, + parameter_block_size) = parameter_jacobian; + } + } + + private: + Program* program_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc new file mode 100644 index 00000000000..328505404d7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/dense_qr_solver.h" + +#include <cstddef> + +#include "Eigen/Dense" +#include "ceres/linear_solver.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +DenseQRSolver::DenseQRSolver(const LinearSolver::Options& options) + : options_(options) {} + +LinearSolver::Summary DenseQRSolver::SolveImpl( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + const int num_rows = A->num_rows(); + const int num_cols = A->num_cols(); + VLOG(2) << "DenseQRSolver: " + << num_rows << " x " << num_cols << " system."; + + if (per_solve_options.D != NULL) { + // Temporarily append a diagonal block to the A matrix, but undo + // it before returning the matrix to the user. + A->AppendDiagonal(per_solve_options.D); + } + + // rhs = [b;0] to account for the additional rows in the lhs. + Vector rhs(num_rows + ((per_solve_options.D !=NULL) ? num_cols : 0)); + rhs.setZero(); + rhs.head(num_rows) = ConstVectorRef(b, num_rows); + + // Solve the system. + VectorRef(x, num_cols) = A->matrix().colPivHouseholderQr().solve(rhs); + + VLOG(3) << "A:\n" << A->matrix(); + VLOG(3) << "x:\n" << VectorRef(x, num_cols); + VLOG(3) << "b:\n" << rhs; + VLOG(3) << "error: " << (A->matrix() * VectorRef(x, num_cols) - rhs).norm(); + + + if (per_solve_options.D != NULL) { + // Undo the modifications to the matrix A. + A->RemoveDiagonal(); + } + + // We always succeed, since the QR solver returns the best solution + // it can. It is the job of the caller to determine if the solution + // is good enough or not. + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = TOLERANCE; + return summary; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h new file mode 100644 index 00000000000..990c8d445eb --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h @@ -0,0 +1,99 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Solve dense rectangular systems Ax = b using the QR factoriztion. +#ifndef CERES_INTERNAL_DENSE_QR_SOLVER_H_ +#define CERES_INTERNAL_DENSE_QR_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class DenseSparseMatrix; + +// This class implements the LinearSolver interface for solving +// rectangular/unsymmetric (well constrained) linear systems of the +// form +// +// Ax = b +// +// Since there does not usually exist a solution that satisfies these +// equations, the solver instead solves the linear least squares +// problem +// +// min_x |Ax - b|^2 +// +// The solution strategy is based on computing the QR decomposition of +// A, i.e. +// +// A = QR +// +// Where Q is an orthonormal matrix and R is an upper triangular +// matrix. Then +// +// Ax = b +// QRx = b +// Q'QRx = Q'b +// Rx = Q'b +// x = R^{-1} Q'b +// +// If the PerSolveOptions struct has a non-null array D, then the +// augmented/regularized linear system +// +// [ A ]x = [b] +// [ diag(D) ] [0] +// +// is solved. +// +// This class uses the dense QR factorization routines from the Eigen +// library. This solver always returns a solution, it is the user's +// responsibility to judge if the solution is good enough for their +// purposes. +class DenseQRSolver: public DenseSparseMatrixSolver { + public: + explicit DenseQRSolver(const LinearSolver::Options& options); + + private: + virtual LinearSolver::Summary SolveImpl( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + const LinearSolver::Options options_; + DISALLOW_COPY_AND_ASSIGN(DenseQRSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_QR_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc new file mode 100644 index 00000000000..5d392ba6c3b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc @@ -0,0 +1,197 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/dense_sparse_matrix.h" + +#include <algorithm> +#include "ceres/matrix_proto.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +DenseSparseMatrix::DenseSparseMatrix(int num_rows, int num_cols) + : has_diagonal_appended_(false), + has_diagonal_reserved_(false) { + // Allocate enough space for the diagonal. + m_.resize(num_rows, num_cols); + m_.setZero(); +} + +DenseSparseMatrix::DenseSparseMatrix(const TripletSparseMatrix& m) + : m_(Eigen::MatrixXd::Zero(m.num_rows(), m.num_cols())), + has_diagonal_appended_(false), + has_diagonal_reserved_(false) { + const double *values = m.values(); + const int *rows = m.rows(); + const int *cols = m.cols(); + int num_nonzeros = m.num_nonzeros(); + + for (int i = 0; i < num_nonzeros; ++i) { + m_(rows[i], cols[i]) += values[i]; + } +} + +DenseSparseMatrix::DenseSparseMatrix(const Matrix& m) + : m_(m), + has_diagonal_appended_(false), + has_diagonal_reserved_(false) { +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +DenseSparseMatrix::DenseSparseMatrix(const SparseMatrixProto& outer_proto) + : m_(Eigen::MatrixXd::Zero( + outer_proto.dense_matrix().num_rows(), + outer_proto.dense_matrix().num_cols())), + has_diagonal_appended_(false), + has_diagonal_reserved_(false) { + const DenseSparseMatrixProto& proto = outer_proto.dense_matrix(); + for (int i = 0; i < m_.rows(); ++i) { + for (int j = 0; j < m_.cols(); ++j) { + m_(i, j) = proto.values(m_.cols() * i + j); + } + } +} +#endif + +void DenseSparseMatrix::SetZero() { + m_.setZero(); +} + +void DenseSparseMatrix::RightMultiply(const double* x, double* y) const { + VectorRef(y, num_rows()) += matrix() * ConstVectorRef(x, num_cols()); +} + +void DenseSparseMatrix::LeftMultiply(const double* x, double* y) const { + VectorRef(y, num_cols()) += + matrix().transpose() * ConstVectorRef(x, num_rows()); +} + +void DenseSparseMatrix::SquaredColumnNorm(double* x) const { + VectorRef(x, num_cols()) = m_.colwise().squaredNorm(); +} + +void DenseSparseMatrix::ScaleColumns(const double* scale) { + m_ *= ConstVectorRef(scale, num_cols()).asDiagonal(); +} + +void DenseSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + *dense_matrix = m_; +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void DenseSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { + CHECK(!has_diagonal_appended_) << "Not supported."; + outer_proto->Clear(); + DenseSparseMatrixProto* proto = outer_proto->mutable_dense_matrix(); + + proto->set_num_rows(num_rows()); + proto->set_num_cols(num_cols()); + + int num_nnz = num_nonzeros(); + for (int i = 0; i < num_nnz; ++i) { + proto->add_values(m_.data()[i]); + } +} +#endif + +void DenseSparseMatrix::AppendDiagonal(double *d) { + CHECK(!has_diagonal_appended_); + if (!has_diagonal_reserved_) { + Matrix tmp = m_; + m_.resize(m_.rows() + m_.cols(), m_.cols()); + m_.setZero(); + m_.block(0, 0, tmp.rows(), tmp.cols()) = tmp; + has_diagonal_reserved_ = true; + } + + m_.bottomLeftCorner(m_.cols(), m_.cols()) = + ConstVectorRef(d, m_.cols()).asDiagonal(); + has_diagonal_appended_ = true; +} + +void DenseSparseMatrix::RemoveDiagonal() { + CHECK(has_diagonal_appended_); + has_diagonal_appended_ = false; + // Leave the diagonal reserved. +} + +int DenseSparseMatrix::num_rows() const { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return m_.rows() - m_.cols(); + } + return m_.rows(); +} + +int DenseSparseMatrix::num_cols() const { + return m_.cols(); +} + +int DenseSparseMatrix::num_nonzeros() const { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return (m_.rows() - m_.cols()) * m_.cols(); + } + return m_.rows() * m_.cols(); +} + +ConstAlignedMatrixRef DenseSparseMatrix::matrix() const { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return ConstAlignedMatrixRef( + m_.data(), m_.rows() - m_.cols(), m_.cols()); + } + return ConstAlignedMatrixRef(m_.data(), m_.rows(), m_.cols()); +} + +AlignedMatrixRef DenseSparseMatrix::mutable_matrix() { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return AlignedMatrixRef( + m_.data(), m_.rows() - m_.cols(), m_.cols()); + } + return AlignedMatrixRef(m_.data(), m_.rows(), m_.cols()); +} + +void DenseSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + const int active_rows = + (has_diagonal_reserved_ && !has_diagonal_appended_) + ? (m_.rows() - m_.cols()) + : m_.rows(); + + for (int r = 0; r < active_rows; ++r) { + for (int c = 0; c < m_.cols(); ++c) { + fprintf(file, "% 10d % 10d %17f\n", r, c, m_(r, c)); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h new file mode 100644 index 00000000000..416c2143c2c --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h @@ -0,0 +1,116 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A dense matrix implemented under the SparseMatrix interface. + +#ifndef CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ + +#include <glog/logging.h> +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; +class TripletSparseMatrix; + +class DenseSparseMatrix : public SparseMatrix { + public: + // Build a matrix with the same content as the TripletSparseMatrix + // m. This assumes that m does not have any repeated entries. + explicit DenseSparseMatrix(const TripletSparseMatrix& m); + explicit DenseSparseMatrix(const Matrix& m); +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit DenseSparseMatrix(const SparseMatrixProto& proto); +#endif + + DenseSparseMatrix(int num_rows, int num_cols); + + virtual ~DenseSparseMatrix() {} + + // SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto* proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + virtual int num_rows() const; + virtual int num_cols() const; + virtual int num_nonzeros() const; + virtual const double* values() const { return m_.data(); } + virtual double* mutable_values() { return m_.data(); } + + ConstAlignedMatrixRef matrix() const; + AlignedMatrixRef mutable_matrix(); + + // Only one diagonal can be appended at a time. The diagonal is appended to + // as a new set of rows, e.g. + // + // Original matrix: + // + // x x x + // x x x + // x x x + // + // After append diagonal (1, 2, 3): + // + // x x x + // x x x + // x x x + // 1 0 0 + // 0 2 0 + // 0 0 3 + // + // Calling RemoveDiagonal removes the block. It is a fatal error to append a + // diagonal to a matrix that already has an appended diagonal, and it is also + // a fatal error to remove a diagonal from a matrix that has none. + void AppendDiagonal(double *d); + void RemoveDiagonal(); + + private: + Matrix m_; + bool has_diagonal_appended_; + bool has_diagonal_reserved_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc new file mode 100644 index 00000000000..e9755043bab --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc @@ -0,0 +1,114 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <glog/logging.h> +#include "ceres/detect_structure.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +void DetectStructure(const CompressedRowBlockStructure& bs, + const int num_eliminate_blocks, + int* row_block_size, + int* e_block_size, + int* f_block_size) { + const int num_row_blocks = bs.rows.size(); + *row_block_size = 0; + *e_block_size = 0; + *f_block_size = 0; + + // Iterate over row blocks of the matrix, checking if row_block, + // e_block or f_block sizes remain constant. + for (int r = 0; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + // We do not care about the sizes of the blocks in rows which do + // not contain e_blocks. + if (row.cells.front().block_id >= num_eliminate_blocks) { + break; + } + const int e_block_id = row.cells.front().block_id; + + if (*row_block_size == 0) { + *row_block_size = row.block.size; + } else if (*row_block_size != Eigen::Dynamic && + *row_block_size != row.block.size) { + *row_block_size = Eigen::Dynamic; + VLOG(2) << "Dynamic row block size because the block size changed from " + << *row_block_size << " to " + << row.block.size; + } + + + if (*e_block_size == 0) { + *e_block_size = bs.cols[e_block_id].size; + } else if (*e_block_size != Eigen::Dynamic && + *e_block_size != bs.cols[e_block_id].size) { + *e_block_size = Eigen::Dynamic; + VLOG(2) << "Dynamic e block size because the block size changed from " + << *e_block_size << " to " + << bs.cols[e_block_id].size; + } + + if (*f_block_size == 0) { + if (row.cells.size() > 1) { + const int f_block_id = row.cells[1].block_id; + *f_block_size = bs.cols[f_block_id].size; + } + } else if (*f_block_size != Eigen::Dynamic) { + for (int c = 1; c < row.cells.size(); ++c) { + if (*f_block_size != bs.cols[row.cells[c].block_id].size) { + *f_block_size = Eigen::Dynamic; + VLOG(2) << "Dynamic f block size because the block size " + << "changed from " << *f_block_size << " to " + << bs.cols[row.cells[c].block_id].size; + break; + } + } + } + + const bool is_everything_dynamic = (*row_block_size == Eigen::Dynamic && + *e_block_size == Eigen::Dynamic && + *f_block_size == Eigen::Dynamic); + if (is_everything_dynamic) { + break; + } + } + + CHECK_NE(*row_block_size, 0) << "No rows found"; + CHECK_NE(*e_block_size, 0) << "No e type blocks found"; + VLOG(1) << "Schur complement static structure <" + << *row_block_size << "," + << *e_block_size << "," + << *f_block_size << ">."; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h new file mode 100644 index 00000000000..8af4f236690 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h @@ -0,0 +1,63 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_DETECT_STRUCTURE_H_ +#define CERES_INTERNAL_DETECT_STRUCTURE_H_ + +#include "ceres/block_structure.h" + +namespace ceres { +namespace internal { + +// Detect static blocks in the problem sparsity. For rows containing +// e_blocks, we are interested in detecting if the size of the row +// blocks, e_blocks and the f_blocks remain constant. If they do, then +// we can use template specialization to improve the performance of +// the block level linear algebra operations used by the +// SchurEliminator. +// +// If a block size is not constant, we return Eigen::Dynamic as the +// value. This just means that the eliminator uses dynamically sized +// linear algebra operations rather than static operations whose size +// is known as compile time. +// +// For more details about e_blocks and f_blocks, see +// schur_complement.h. This information is used to initialized an +// appropriate template specialization of SchurEliminator. +void DetectStructure(const CompressedRowBlockStructure& bs, + const int num_eliminate_blocks, + int* row_block_size, + int* e_block_size, + int* f_block_size); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DETECT_STRUCTURE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc b/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc new file mode 100644 index 00000000000..ea05aefec8c --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc @@ -0,0 +1,71 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include <glog/logging.h> +#include "ceres/evaluator.h" +#include "ceres/block_evaluate_preparer.h" +#include "ceres/block_jacobian_writer.h" +#include "ceres/compressed_row_jacobian_writer.h" +#include "ceres/scratch_evaluate_preparer.h" +#include "ceres/dense_jacobian_writer.h" +#include "ceres/program_evaluator.h" + +namespace ceres { +namespace internal { + +Evaluator::~Evaluator() {} + +Evaluator* Evaluator::Create(const Evaluator::Options& options, + Program* program, + string* error) { + switch (options.linear_solver_type) { + case DENSE_QR: + return new ProgramEvaluator<ScratchEvaluatePreparer, + DenseJacobianWriter>(options, + program); + case DENSE_SCHUR: + case SPARSE_SCHUR: + case ITERATIVE_SCHUR: + case CGNR: + return new ProgramEvaluator<BlockEvaluatePreparer, + BlockJacobianWriter>(options, + program); + case SPARSE_NORMAL_CHOLESKY: + return new ProgramEvaluator<ScratchEvaluatePreparer, + CompressedRowJacobianWriter>(options, + program); + default: + *error = "Invalid Linear Solver Type. Unable to create evaluator."; + return NULL; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/evaluator.h b/extern/libmv/third_party/ceres/internal/ceres/evaluator.h new file mode 100644 index 00000000000..adefdd26660 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/evaluator.h @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_EVALUATOR_H_ +#define CERES_INTERNAL_EVALUATOR_H_ + +#include <string> +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class Program; +class SparseMatrix; + +// The Evaluator interface offers a way to interact with a least squares cost +// function that is useful for an optimizer that wants to minimize the least +// squares objective. This insulates the optimizer from issues like Jacobian +// storage, parameterization, etc. +class Evaluator { + public: + virtual ~Evaluator(); + + struct Options { + Options() + : num_threads(1), + num_eliminate_blocks(-1), + linear_solver_type(DENSE_QR) {} + + int num_threads; + int num_eliminate_blocks; + LinearSolverType linear_solver_type; + }; + + static Evaluator* Create(const Options& options, + Program* program, + string* error); + + // Build and return a sparse matrix for storing and working with the Jacobian + // of the objective function. The jacobian has dimensions + // NumEffectiveParameters() by NumParameters(), and is typically extremely + // sparse. Since the sparsity pattern of the Jacobian remains constant over + // the lifetime of the optimization problem, this method is used to + // instantiate a SparseMatrix object with the appropriate sparsity structure + // (which can be an expensive operation) and then reused by the optimization + // algorithm and the various linear solvers. + // + // It is expected that the classes implementing this interface will be aware + // of their client's requirements for the kind of sparse matrix storage and + // layout that is needed for an efficient implementation. For example + // CompressedRowOptimizationProblem creates a compressed row representation of + // the jacobian for use with CHOLMOD, where as BlockOptimizationProblem + // creates a BlockSparseMatrix representation of the jacobian for use in the + // Schur complement based methods. + virtual SparseMatrix* CreateJacobian() const = 0; + + // Evaluate the cost function for the given state. Returns the cost, + // residuals, and jacobian in the corresponding arguments. Both residuals and + // jacobian are optional; to avoid computing them, pass NULL. + // + // If non-NULL, the Jacobian must have a suitable sparsity pattern; only the + // values array of the jacobian is modified. + // + // state is an array of size NumParameters(), cost is a pointer to a single + // double, and residuals is an array of doubles of size NumResiduals(). + virtual bool Evaluate(const double* state, + double* cost, + double* residuals, + SparseMatrix* jacobian) = 0; + + // Make a change delta (of size NumEffectiveParameters()) to state (of size + // NumParameters()) and store the result in state_plus_delta. + // + // In the case that there are no parameterizations used, this is equivalent to + // + // state_plus_delta[i] = state[i] + delta[i] ; + // + // however, the mapping is more complicated in the case of parameterizations + // like quaternions. This is the same as the "Plus()" operation in + // local_parameterization.h, but operating over the entire state vector for a + // problem. + virtual bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const = 0; + + // The number of parameters in the optimization problem. + virtual int NumParameters() const = 0; + + // This is the effective number of parameters that the optimizer may adjust. + // This applies when there are parameterizations on some of the parameters. + virtual int NumEffectiveParameters() const = 0; + + // The number of residuals in the optimization problem. + virtual int NumResiduals() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_EVALUATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/file.cc b/extern/libmv/third_party/ceres/internal/ceres/file.cc new file mode 100644 index 00000000000..5fc9d220861 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/file.cc @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Really simple file IO. + +#include <cstdio> +#include <glog/logging.h> + +namespace ceres { +namespace internal { + +using std::string; + +void WriteStringToFileOrDie(const string &data, const string &filename) { + FILE* file_descriptor = fopen(filename.c_str(), "wb"); + if (!file_descriptor) { + LOG(FATAL) << "Couldn't write to file: " << filename; + } + fwrite(data.c_str(), 1, data.size(), file_descriptor); + fclose(file_descriptor); +} + +void ReadFileToStringOrDie(const string &filename, string *data) { + FILE* file_descriptor = file_descriptor = fopen(filename.c_str(), "r"); + + if (!file_descriptor) { + LOG(FATAL) << "Couldn't read file: " << filename; + } + + // Resize the input buffer appropriately. + fseek(file_descriptor, 0L, SEEK_END); + int num_bytes = ftell(file_descriptor); + data->resize(num_bytes); + + // Read the data. + fseek(file_descriptor, 0L, SEEK_SET); + int num_read = fread(&((*data)[0]), + sizeof((*data)[0]), + num_bytes, + file_descriptor); + if (num_read != num_bytes) { + LOG(FATAL) << "Couldn't read all of " << filename + << "expected bytes: " << num_bytes * sizeof((*data)[0]) + << "actual bytes: " << num_read; + } + fclose(file_descriptor); +} + +string JoinPath(const string& dirname, const string& basename) { +#ifdef _WIN32 + static const char separator = '\\'; +#else + static const char separator = '/'; +#endif // _WIN32 + + if ((!basename.empty() && basename[0] == separator) || dirname.empty()) { + return basename; + } else if (dirname[dirname.size() - 1] == separator) { + return dirname + basename; + } else { + return dirname + string(&separator, 1) + basename; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/file.h b/extern/libmv/third_party/ceres/internal/ceres/file.h new file mode 100644 index 00000000000..4741d650646 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/file.h @@ -0,0 +1,52 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Simple file IO support. This is a portability shim. + +#ifndef CERES_INTERNAL_FILE_H_ +#define CERES_INTERNAL_FILE_H_ + +#include <string> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +void WriteStringToFileOrDie(const string &data, const string &filename); +void ReadFileToStringOrDie(const string &filename, string *data); + +// Join two path components, adding a slash if necessary. If basename is an +// absolute path then JoinPath ignores dirname and simply returns basename. +string JoinPath(const string& dirname, const string& basename); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_FILE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc new file mode 100644 index 00000000000..5529386e485 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 2>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc new file mode 100644 index 00000000000..fd7af95192e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc new file mode 100644 index 00000000000..109483e9fc0 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc new file mode 100644 index 00000000000..b93e82fe2fa --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc new file mode 100644 index 00000000000..86352c07304 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc new file mode 100644 index 00000000000..200df7f5931 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc new file mode 100644 index 00000000000..1fda3434bef --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 9>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc new file mode 100644 index 00000000000..385cd2d70c9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc new file mode 100644 index 00000000000..7b15d6366ac --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc new file mode 100644 index 00000000000..29a610d743e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc new file mode 100644 index 00000000000..a3bc4dc6f83 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc new file mode 100644 index 00000000000..f71a4f62944 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 2>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc new file mode 100644 index 00000000000..52259fb1a67 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc new file mode 100644 index 00000000000..775424e6c8f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc new file mode 100644 index 00000000000..97cde594059 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc new file mode 100644 index 00000000000..4cba32e26c8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<Dynamic, Dynamic, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc new file mode 100644 index 00000000000..abba40824ef --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc @@ -0,0 +1,308 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/gradient_checking_cost_function.h" + +#include <algorithm> +#include <cmath> +#include <numeric> +#include <string> +#include <vector> + +#include <glog/logging.h> +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/runtime_numeric_diff_cost_function.h" +#include "ceres/stringprintf.h" +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/problem.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { +namespace { + +// True if x and y have an absolute relative difference less than +// relative_precision and false otherwise. Stores the relative and absolute +// difference in relative/absolute_error if non-NULL. +bool IsClose(double x, double y, double relative_precision, + double *relative_error, + double *absolute_error) { + double local_absolute_error; + double local_relative_error; + if (!absolute_error) { + absolute_error = &local_absolute_error; + } + if (!relative_error) { + relative_error = &local_relative_error; + } + *absolute_error = fabs(x - y); + *relative_error = *absolute_error / max(fabs(x), fabs(y)); + if (x == 0 || y == 0) { + // If x or y is exactly zero, then relative difference doesn't have any + // meaning. Take the absolute difference instead. + *relative_error = *absolute_error; + } + return fabs(*relative_error) < fabs(relative_precision); +} + +class GradientCheckingCostFunction : public CostFunction { + public: + GradientCheckingCostFunction(const CostFunction* function, + double relative_step_size, + double relative_precision, + const string& extra_info) + : function_(function), + finite_diff_cost_function_( + CreateRuntimeNumericDiffCostFunction(function, + CENTRAL, + relative_step_size)), + relative_precision_(relative_precision), + extra_info_(extra_info) { + *mutable_parameter_block_sizes() = function->parameter_block_sizes(); + set_num_residuals(function->num_residuals()); + } + + virtual ~GradientCheckingCostFunction() { } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + if (!jacobians) { + // Nothing to check in this case; just forward. + return function_->Evaluate(parameters, residuals, NULL); + } + + int num_residuals = function_->num_residuals(); + + // Make space for the jacobians of the two methods. + const vector<int16>& block_sizes = function_->parameter_block_sizes(); + vector<Matrix> term_jacobians(block_sizes.size()); + vector<Matrix> finite_difference_jacobians(block_sizes.size()); + vector<double*> term_jacobian_pointers(block_sizes.size()); + vector<double*> finite_difference_jacobian_pointers(block_sizes.size()); + for (int i = 0; i < block_sizes.size(); i++) { + term_jacobians[i].resize(num_residuals, block_sizes[i]); + term_jacobian_pointers[i] = term_jacobians[i].data(); + finite_difference_jacobians[i].resize(num_residuals, block_sizes[i]); + finite_difference_jacobian_pointers[i] = + finite_difference_jacobians[i].data(); + } + + // Evaluate the derivative using the user supplied code. + if (!function_->Evaluate(parameters, + residuals, + &term_jacobian_pointers[0])) { + LOG(WARNING) << "Function evaluation failed."; + return false; + } + + // Evaluate the derivative using numeric derivatives. + finite_diff_cost_function_->Evaluate( + parameters, + residuals, + &finite_difference_jacobian_pointers[0]); + + // See if any elements have relative error larger than the threshold. + int num_bad_jacobian_components = 0; + double worst_relative_error = 0; + + // Accumulate the error message for all the jacobians, since it won't get + // output if there are no bad jacobian components. + string m; + for (int k = 0; k < block_sizes.size(); k++) { + // Copy the original jacobian blocks into the jacobians array. + if (jacobians[k] != NULL) { + MatrixRef(jacobians[k], + term_jacobians[k].rows(), + term_jacobians[k].cols()) = term_jacobians[k]; + } + + StringAppendF(&m, + "========== " + "Jacobian for " "block %d: (%ld by %ld)) " + "==========\n", + k, + term_jacobians[k].rows(), + term_jacobians[k].cols()); + // The funny spacing creates appropriately aligned column headers. + m += " block row col user dx/dy num diff dx/dy " + "abs error relative error parameter residual\n"; + + for (int i = 0; i < term_jacobians[k].rows(); i++) { + for (int j = 0; j < term_jacobians[k].cols(); j++) { + double term_jacobian = term_jacobians[k](i, j); + double finite_jacobian = finite_difference_jacobians[k](i, j); + double relative_error, absolute_error; + bool bad_jacobian_entry = + !IsClose(term_jacobian, + finite_jacobian, + relative_precision_, + &relative_error, + &absolute_error); + worst_relative_error = std::max(worst_relative_error, + relative_error); + + StringAppendF(&m, "%6d %4d %4d %17g %17g %17g %17g %17g %17g", + k, i, j, + term_jacobian, finite_jacobian, + absolute_error, relative_error, + parameters[k][j], + residuals[i]); + + if (bad_jacobian_entry) { + num_bad_jacobian_components++; + StringAppendF( + &m, " ------ (%d,%d,%d) Relative error worse than %g", + k, i, j, relative_precision_); + } + m += "\n"; + } + } + } + + // Since there were some bad errors, dump comprehensive debug info. + if (num_bad_jacobian_components) { + string header = StringPrintf("Detected %d bad jacobian component(s). " + "Worst relative error was %g.\n", + num_bad_jacobian_components, + worst_relative_error); + if (!extra_info_.empty()) { + header += "Extra info for this residual: " + extra_info_ + "\n"; + } + LOG(WARNING) << "\n" << header << m; + } + return true; + } + + private: + const CostFunction* function_; + internal::scoped_ptr<CostFunction> finite_diff_cost_function_; + double relative_precision_; + string extra_info_; +}; + +} // namespace + +CostFunction *CreateGradientCheckingCostFunction( + const CostFunction *cost_function, + double relative_step_size, + double relative_precision, + const string& extra_info) { + return new GradientCheckingCostFunction(cost_function, + relative_step_size, + relative_precision, + extra_info); +} + +ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision) { + // We create new CostFunctions by wrapping the original CostFunction + // in a gradient checking CostFunction. So its okay for the + // ProblemImpl to take ownership of it and destroy it. The + // LossFunctions and LocalParameterizations are reused and since + // they are owned by problem_impl, gradient_checking_problem_impl + // should not take ownership of it. + Problem::Options gradient_checking_problem_options; + gradient_checking_problem_options.cost_function_ownership = TAKE_OWNERSHIP; + gradient_checking_problem_options.loss_function_ownership = + DO_NOT_TAKE_OWNERSHIP; + gradient_checking_problem_options.local_parameterization_ownership = + DO_NOT_TAKE_OWNERSHIP; + + ProblemImpl* gradient_checking_problem_impl = new ProblemImpl( + gradient_checking_problem_options); + + Program* program = problem_impl->mutable_program(); + + // For every ParameterBlock in problem_impl, create a new parameter + // block with the same local parameterization and constancy. + const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + gradient_checking_problem_impl->AddParameterBlock( + parameter_block->mutable_user_state(), + parameter_block->Size(), + parameter_block->mutable_local_parameterization()); + + if (parameter_block->IsConstant()) { + gradient_checking_problem_impl->SetParameterBlockConstant( + parameter_block->mutable_user_state()); + } + } + + // For every ResidualBlock in problem_impl, create a new + // ResidualBlock by wrapping its CostFunction inside a + // GradientCheckingCostFunction. + const vector<ResidualBlock*>& residual_blocks = program->residual_blocks(); + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + + // Build a human readable string which identifies the + // ResidualBlock. This is used by the GradientCheckingCostFunction + // when logging debugging information. + string extra_info = StringPrintf( + "Residual block id %d; depends on parameters [", i); + vector<double*> parameter_blocks; + for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + parameter_blocks.push_back(parameter_block->mutable_user_state()); + StringAppendF(&extra_info, "%p", parameter_block->mutable_user_state()); + extra_info += (j < residual_block->NumParameterBlocks() - 1) ? ", " : "]"; + } + + // Wrap the original CostFunction in a GradientCheckingCostFunction. + CostFunction* gradient_checking_cost_function = + CreateGradientCheckingCostFunction(residual_block->cost_function(), + relative_step_size, + relative_precision, + extra_info); + + // The const_cast is necessary because + // ProblemImpl::AddResidualBlock can potentially take ownership of + // the LossFunction, but in this case we are guaranteed that this + // will not be the case, so this const_cast is harmless. + gradient_checking_problem_impl->AddResidualBlock( + gradient_checking_cost_function, + const_cast<LossFunction*>(residual_block->loss_function()), + parameter_blocks); + } + + return gradient_checking_problem_impl; +} + + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.h b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.h new file mode 100644 index 00000000000..d49c8e6c244 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.h @@ -0,0 +1,85 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ +#define CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ + +#include <string> + +#include "ceres/cost_function.h" + +namespace ceres { +namespace internal { + +class ProblemImpl; + +// Creates a CostFunction that checks the jacobians that cost_function computes +// with finite differences. Bad results are logged; required precision is +// controlled by relative_precision and the numeric differentiation step size is +// controlled with relative_step_size. See solver.h for a better explanation of +// relative_step_size. Caller owns result. +// +// The condition enforced is that +// +// (J_actual(i, j) - J_numeric(i, j)) +// ------------------------------------ < relative_precision +// max(J_actual(i, j), J_numeric(i, j)) +// +// where J_actual(i, j) is the jacobian as computed by the supplied cost +// function (by the user) and J_numeric is the jacobian as computed by finite +// differences. +// +// Note: This is quite inefficient and is intended only for debugging. +CostFunction* CreateGradientCheckingCostFunction( + const CostFunction* cost_function, + double relative_step_size, + double relative_precision, + const string& extra_info); + +// Create a new ProblemImpl object from the input problem_impl, where +// each CostFunctions in problem_impl are wrapped inside a +// GradientCheckingCostFunctions. This gives us a ProblemImpl object +// which checks its derivatives against estimates from numeric +// differentiation everytime a ResidualBlock is evaluated. +// +// relative_step_size and relative_precision are parameters to control +// the numeric differentiation and the relative tolerance between the +// jacobian computed by the CostFunctions in problem_impl and +// jacobians obtained by numerically differentiating them. For more +// details see the documentation for +// CreateGradientCheckingCostFunction above. +ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/graph.h b/extern/libmv/third_party/ceres/internal/ceres/graph.h new file mode 100644 index 00000000000..fd7a224f0aa --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/graph.h @@ -0,0 +1,138 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_GRAPH_H_ +#define CERES_INTERNAL_GRAPH_H_ + +#include <limits> +#include <glog/logging.h> +#include "ceres/integral_types.h" +#include "ceres/map_util.h" +#include "ceres/collections_port.h" +#include "ceres/internal/macros.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// A weighted undirected graph templated over the vertex ids. Vertex +// should be hashable and comparable. +template <typename Vertex> +class Graph { + public: + Graph() {} + + // Add a weighted vertex. If the vertex already exists in the graph, + // its weight is set to the new weight. + void AddVertex(const Vertex& vertex, double weight) { + if (vertices_.find(vertex) == vertices_.end()) { + vertices_.insert(vertex); + edges_[vertex] = HashSet<Vertex>(); + } + vertex_weights_[vertex] = weight; + } + + // Uses weight = 1.0. If vertex already exists, its weight is set to + // 1.0. + void AddVertex(const Vertex& vertex) { + AddVertex(vertex, 1.0); + } + + // Add a weighted edge between the vertex1 and vertex2. Calling + // AddEdge on a pair of vertices which do not exist in the graph yet + // will result in undefined behavior. + // + // It is legal to call this method repeatedly for the same set of + // vertices. + void AddEdge(const Vertex& vertex1, const Vertex& vertex2, double weight) { + DCHECK(vertices_.find(vertex1) != vertices_.end()); + DCHECK(vertices_.find(vertex2) != vertices_.end()); + + if (edges_[vertex1].insert(vertex2).second) { + edges_[vertex2].insert(vertex1); + } + + if (vertex1 < vertex2) { + edge_weights_[make_pair(vertex1, vertex2)] = weight; + } else { + edge_weights_[make_pair(vertex2, vertex1)] = weight; + } + } + + // Uses weight = 1.0. + void AddEdge(const Vertex& vertex1, const Vertex& vertex2) { + AddEdge(vertex1, vertex2, 1.0); + } + + // Calling VertexWeight on a vertex not in the graph will result in + // undefined behavior. + double VertexWeight(const Vertex& vertex) const { + return FindOrDie(vertex_weights_, vertex); + } + + // Calling EdgeWeight on a pair of vertices where either one of the + // vertices is not present in the graph will result in undefined + // behaviour. If there is no edge connecting vertex1 and vertex2, + // the edge weight is zero. + double EdgeWeight(const Vertex& vertex1, const Vertex& vertex2) const { + if (vertex1 < vertex2) { + return FindWithDefault(edge_weights_, make_pair(vertex1, vertex2), 0.0); + } else { + return FindWithDefault(edge_weights_, make_pair(vertex2, vertex1), 0.0); + } + } + + // Calling Neighbors on a vertex not in the graph will result in + // undefined behaviour. + const HashSet<Vertex>& Neighbors(const Vertex& vertex) const { + return FindOrDie(edges_, vertex); + } + + const HashSet<Vertex>& vertices() const { + return vertices_; + } + + static double InvalidWeight() { + return std::numeric_limits<double>::quiet_NaN(); + }; + + private: + HashSet<Vertex> vertices_; + HashMap<Vertex, double> vertex_weights_; + HashMap<Vertex, HashSet<Vertex> > edges_; + HashMap<pair<Vertex, Vertex>, double> edge_weights_; + + DISALLOW_COPY_AND_ASSIGN(Graph); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRAPH_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h b/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h new file mode 100644 index 00000000000..3b42d936336 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h @@ -0,0 +1,270 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Various algorithms that operate on undirected graphs. + +#ifndef CERES_INTERNAL_GRAPH_ALGORITHMS_H_ +#define CERES_INTERNAL_GRAPH_ALGORITHMS_H_ + +#include <vector> +#include <glog/logging.h> +#include "ceres/collections_port.h" +#include "ceres/graph.h" + +namespace ceres { +namespace internal { + +// Compare two vertices of a graph by their degrees. +template <typename Vertex> +class VertexDegreeLessThan { + public: + explicit VertexDegreeLessThan(const Graph<Vertex>& graph) + : graph_(graph) {} + + bool operator()(const Vertex& lhs, const Vertex& rhs) const { + if (graph_.Neighbors(lhs).size() == graph_.Neighbors(rhs).size()) { + return lhs < rhs; + } + return graph_.Neighbors(lhs).size() < graph_.Neighbors(rhs).size(); + } + + private: + const Graph<Vertex>& graph_; +}; + +// Order the vertices of a graph using its (approximately) largest +// independent set, where an independent set of a graph is a set of +// vertices that have no edges connecting them. The maximum +// independent set problem is NP-Hard, but there are effective +// approximation algorithms available. The implementation here uses a +// breadth first search that explores the vertices in order of +// increasing degree. The same idea is used by Saad & Li in "MIQR: A +// multilevel incomplete QR preconditioner for large sparse +// least-squares problems", SIMAX, 2007. +// +// Given a undirected graph G(V,E), the algorithm is a greedy BFS +// search where the vertices are explored in increasing order of their +// degree. The output vector ordering contains elements of S in +// increasing order of their degree, followed by elements of V - S in +// increasing order of degree. The return value of the function is the +// cardinality of S. +template <typename Vertex> +int IndependentSetOrdering(const Graph<Vertex>& graph, + vector<Vertex>* ordering) { + const HashSet<Vertex>& vertices = graph.vertices(); + const int num_vertices = vertices.size(); + + CHECK_NOTNULL(ordering); + ordering->clear(); + ordering->reserve(num_vertices); + + // Colors for labeling the graph during the BFS. + const char kWhite = 0; + const char kGrey = 1; + const char kBlack = 2; + + // Mark all vertices white. + HashMap<Vertex, char> vertex_color; + vector<Vertex> vertex_queue; + for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); + it != vertices.end(); + ++it) { + vertex_color[*it] = kWhite; + vertex_queue.push_back(*it); + } + + + sort(vertex_queue.begin(), vertex_queue.end(), + VertexDegreeLessThan<Vertex>(graph)); + + // Iterate over vertex_queue. Pick the first white vertex, add it + // to the independent set. Mark it black and its neighbors grey. + for (int i = 0; i < vertex_queue.size(); ++i) { + const Vertex& vertex = vertex_queue[i]; + if (vertex_color[vertex] != kWhite) { + continue; + } + + ordering->push_back(vertex); + vertex_color[vertex] = kBlack; + const HashSet<Vertex>& neighbors = graph.Neighbors(vertex); + for (typename HashSet<Vertex>::const_iterator it = neighbors.begin(); + it != neighbors.end(); + ++it) { + vertex_color[*it] = kGrey; + } + } + + int independent_set_size = ordering->size(); + + // Iterate over the vertices and add all the grey vertices to the + // ordering. At this stage there should only be black or grey + // vertices in the graph. + for (typename vector<Vertex>::const_iterator it = vertex_queue.begin(); + it != vertex_queue.end(); + ++it) { + const Vertex vertex = *it; + DCHECK(vertex_color[vertex] != kWhite); + if (vertex_color[vertex] != kBlack) { + ordering->push_back(vertex); + } + } + + CHECK_EQ(ordering->size(), num_vertices); + return independent_set_size; +} + +// Find the connected component for a vertex implemented using the +// find and update operation for disjoint-set. Recursively traverse +// the disjoint set structure till you reach a vertex whose connected +// component has the same id as the vertex itself. Along the way +// update the connected components of all the vertices. This updating +// is what gives this data structure its efficiency. +template <typename Vertex> +Vertex FindConnectedComponent(const Vertex& vertex, + HashMap<Vertex, Vertex>* union_find) { + typename HashMap<Vertex, Vertex>::iterator it = union_find->find(vertex); + DCHECK(it != union_find->end()); + if (it->second != vertex) { + it->second = FindConnectedComponent(it->second, union_find); + } + + return it->second; +} + +// Compute a degree two constrained Maximum Spanning Tree/forest of +// the input graph. Caller owns the result. +// +// Finding degree 2 spanning tree of a graph is not always +// possible. For example a star graph, i.e. a graph with n-nodes +// where one node is connected to the other n-1 nodes does not have +// a any spanning trees of degree less than n-1.Even if such a tree +// exists, finding such a tree is NP-Hard. + +// We get around both of these problems by using a greedy, degree +// constrained variant of Kruskal's algorithm. We start with a graph +// G_T with the same vertex set V as the input graph G(V,E) but an +// empty edge set. We then iterate over the edges of G in decreasing +// order of weight, adding them to G_T if doing so does not create a +// cycle in G_T} and the degree of all the vertices in G_T remains +// bounded by two. This O(|E|) algorithm results in a degree-2 +// spanning forest, or a collection of linear paths that span the +// graph G. +template <typename Vertex> +Graph<Vertex>* +Degree2MaximumSpanningForest(const Graph<Vertex>& graph) { + // Array of edges sorted in decreasing order of their weights. + vector<pair<double, pair<Vertex, Vertex> > > weighted_edges; + Graph<Vertex>* forest = new Graph<Vertex>(); + + // Disjoint-set to keep track of the connected components in the + // maximum spanning tree. + HashMap<Vertex, Vertex> disjoint_set; + + // Sort of the edges in the graph in decreasing order of their + // weight. Also add the vertices of the graph to the Maximum + // Spanning Tree graph and set each vertex to be its own connected + // component in the disjoint_set structure. + const HashSet<Vertex>& vertices = graph.vertices(); + for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); + it != vertices.end(); + ++it) { + const Vertex vertex1 = *it; + forest->AddVertex(vertex1, graph.VertexWeight(vertex1)); + disjoint_set[vertex1] = vertex1; + + const HashSet<Vertex>& neighbors = graph.Neighbors(vertex1); + for (typename HashSet<Vertex>::const_iterator it2 = neighbors.begin(); + it2 != neighbors.end(); + ++it2) { + const Vertex vertex2 = *it2; + if (vertex1 >= vertex2) { + continue; + } + const double weight = graph.EdgeWeight(vertex1, vertex2); + weighted_edges.push_back(make_pair(weight, make_pair(vertex1, vertex2))); + } + } + + // The elements of this vector, are pairs<edge_weight, + // edge>. Sorting it using the reverse iterators gives us the edges + // in decreasing order of edges. + sort(weighted_edges.rbegin(), weighted_edges.rend()); + + // Greedily add edges to the spanning tree/forest as long as they do + // not violate the degree/cycle constraint. + for (int i =0; i < weighted_edges.size(); ++i) { + const pair<Vertex, Vertex>& edge = weighted_edges[i].second; + const Vertex vertex1 = edge.first; + const Vertex vertex2 = edge.second; + + // Check if either of the vertices are of degree 2 already, in + // which case adding this edge will violate the degree 2 + // constraint. + if ((forest->Neighbors(vertex1).size() == 2) || + (forest->Neighbors(vertex2).size() == 2)) { + continue; + } + + // Find the id of the connected component to which the two + // vertices belong to. If the id is the same, it means that the + // two of them are already connected to each other via some other + // vertex, and adding this edge will create a cycle. + Vertex root1 = FindConnectedComponent(vertex1, &disjoint_set); + Vertex root2 = FindConnectedComponent(vertex2, &disjoint_set); + + if (root1 == root2) { + continue; + } + + // This edge can be added, add an edge in either direction with + // the same weight as the original graph. + const double edge_weight = graph.EdgeWeight(vertex1, vertex2); + forest->AddEdge(vertex1, vertex2, edge_weight); + forest->AddEdge(vertex2, vertex1, edge_weight); + + // Connected the two connected components by updating the + // disjoint_set structure. Always connect the connected component + // with the greater index with the connected component with the + // smaller index. This should ensure shallower trees, for quicker + // lookup. + if (root2 < root1) { + std::swap(root1, root2); + }; + + disjoint_set[root2] = root1; + } + return forest; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRAPH_ALGORITHMS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc new file mode 100644 index 00000000000..bd908846362 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc @@ -0,0 +1,237 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/implicit_schur_complement.h" + +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +ImplicitSchurComplement::ImplicitSchurComplement(int num_eliminate_blocks, + bool constant_sparsity, + bool preconditioner) + : num_eliminate_blocks_(num_eliminate_blocks), + constant_sparsity_(constant_sparsity), + preconditioner_(preconditioner), + A_(NULL), + D_(NULL), + b_(NULL), + block_diagonal_EtE_inverse_(NULL), + block_diagonal_FtF_inverse_(NULL) { +} + +ImplicitSchurComplement::~ImplicitSchurComplement() { +} + +void ImplicitSchurComplement::Init(const BlockSparseMatrixBase& A, + const double* D, + const double* b) { + // Since initialization is reasonably heavy, perhaps we can save on + // constructing a new object everytime. + if ((A_ == NULL) || !constant_sparsity_) { + A_.reset(new PartitionedMatrixView(A, num_eliminate_blocks_)); + } + + D_ = D; + b_ = b; + + // Initialize temporary storage and compute the block diagonals of + // E'E and F'E. + if ((!constant_sparsity_) || (block_diagonal_EtE_inverse_ == NULL)) { + block_diagonal_EtE_inverse_.reset(A_->CreateBlockDiagonalEtE()); + if (preconditioner_) { + block_diagonal_FtF_inverse_.reset(A_->CreateBlockDiagonalFtF()); + } + rhs_.resize(A_->num_cols_f()); + rhs_.setZero(); + tmp_rows_.resize(A_->num_rows()); + tmp_e_cols_.resize(A_->num_cols_e()); + tmp_e_cols_2_.resize(A_->num_cols_e()); + tmp_f_cols_.resize(A_->num_cols_f()); + } else { + A_->UpdateBlockDiagonalEtE(block_diagonal_EtE_inverse_.get()); + if (preconditioner_) { + A_->UpdateBlockDiagonalFtF(block_diagonal_FtF_inverse_.get()); + } + } + + // The block diagonals of the augmented linear system contain + // contributions from the diagonal D if it is non-null. Add that to + // the block diagonals and invert them. + if (D_ != NULL) { + AddDiagonalAndInvert(D_, block_diagonal_EtE_inverse_.get()); + if (preconditioner_) { + AddDiagonalAndInvert(D_ + A_->num_cols_e(), + block_diagonal_FtF_inverse_.get()); + } + } else { + AddDiagonalAndInvert(NULL, block_diagonal_EtE_inverse_.get()); + if (preconditioner_) { + AddDiagonalAndInvert(NULL, block_diagonal_FtF_inverse_.get()); + } + } + + // Compute the RHS of the Schur complement system. + UpdateRhs(); +} + +// Evaluate the product +// +// Sx = [F'F - F'E (E'E)^-1 E'F]x +// +// By breaking it down into individual matrix vector products +// involving the matrices E and F. This is implemented using a +// PartitionedMatrixView of the input matrix A. +void ImplicitSchurComplement::RightMultiply(const double* x, double* y) const { + // y1 = F x + tmp_rows_.setZero(); + A_->RightMultiplyF(x, tmp_rows_.data()); + + // y2 = E' y1 + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); + + // y3 = -(E'E)^-1 y2 + tmp_e_cols_2_.setZero(); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), + tmp_e_cols_2_.data()); + tmp_e_cols_2_ *= -1.0; + + // y1 = y1 + E y3 + A_->RightMultiplyE(tmp_e_cols_2_.data(), tmp_rows_.data()); + + // y5 = D * x + if (D_ != NULL) { + ConstVectorRef Dref(D_ + A_->num_cols_e(), num_cols()); + VectorRef(y, num_cols()) = + (Dref.array().square() * + ConstVectorRef(x, num_cols()).array()).matrix(); + } else { + VectorRef(y, num_cols()).setZero(); + } + + // y = y5 + F' y1 + A_->LeftMultiplyF(tmp_rows_.data(), y); +} + +// Given a block diagonal matrix and an optional array of diagonal +// entries D, add them to the diagonal of the matrix and compute the +// inverse of each diagonal block. +void ImplicitSchurComplement::AddDiagonalAndInvert( + const double* D, + BlockSparseMatrix* block_diagonal) { + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + for (int r = 0; r < block_diagonal_structure->rows.size(); ++r) { + const int row_block_pos = block_diagonal_structure->rows[r].block.position; + const int row_block_size = block_diagonal_structure->rows[r].block.size; + const Cell& cell = block_diagonal_structure->rows[r].cells[0]; + MatrixRef m(block_diagonal->mutable_values() + cell.position, + row_block_size, row_block_size); + + if (D != NULL) { + ConstVectorRef d(D + row_block_pos, row_block_size); + m += d.array().square().matrix().asDiagonal(); + } + + m = m + .selfadjointView<Eigen::Upper>() + .ldlt() + .solve(Matrix::Identity(row_block_size, row_block_size)); + } +} + +// Similar to RightMultiply, use the block structure of the matrix A +// to compute y = (E'E)^-1 (E'b - E'F x). +void ImplicitSchurComplement::BackSubstitute(const double* x, double* y) { + const int num_cols_e = A_->num_cols_e(); + const int num_cols_f = A_->num_cols_f(); + const int num_cols = A_->num_cols(); + const int num_rows = A_->num_rows(); + + // y1 = F x + tmp_rows_.setZero(); + A_->RightMultiplyF(x, tmp_rows_.data()); + + // y2 = b - y1 + tmp_rows_ = ConstVectorRef(b_, num_rows) - tmp_rows_; + + // y3 = E' y2 + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); + + // y = (E'E)^-1 y3 + VectorRef(y, num_cols).setZero(); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y); + + // The full solution vector y has two blocks. The first block of + // variables corresponds to the eliminated variables, which we just + // computed via back substitution. The second block of variables + // corresponds to the Schur complement system, so we just copy those + // values from the solution to the Schur complement. + VectorRef(y + num_cols_e, num_cols_f) = ConstVectorRef(x, num_cols_f); +} + +// Compute the RHS of the Schur complement system. +// +// rhs = F'b - F'E (E'E)^-1 E'b +// +// Like BackSubstitute, we use the block structure of A to implement +// this using a series of matrix vector products. +void ImplicitSchurComplement::UpdateRhs() { + // y1 = E'b + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(b_, tmp_e_cols_.data()); + + // y2 = (E'E)^-1 y1 + Vector y2 = Vector::Zero(A_->num_cols_e()); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y2.data()); + + // y3 = E y2 + tmp_rows_.setZero(); + A_->RightMultiplyE(y2.data(), tmp_rows_.data()); + + // y3 = b - y3 + tmp_rows_ = ConstVectorRef(b_, A_->num_rows()) - tmp_rows_; + + // rhs = F' y3 + rhs_.setZero(); + A_->LeftMultiplyF(tmp_rows_.data(), rhs_.data()); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h new file mode 100644 index 00000000000..37a319f9c57 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h @@ -0,0 +1,176 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// An iterative solver for solving the Schur complement/reduced camera +// linear system that arise in SfM problems. + +#ifndef CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ +#define CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ + +#include "ceres/linear_operator.h" +#include "ceres/partitioned_matrix_view.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; +class BlockSparseMatrixBase; + +// This class implements various linear algebraic operations related +// to the Schur complement without explicitly forming it. +// +// +// Given a reactangular linear system Ax = b, where +// +// A = [E F] +// +// The normal equations are given by +// +// A'Ax = A'b +// +// |E'E E'F||y| = |E'b| +// |F'E F'F||z| |F'b| +// +// and the Schur complement system is given by +// +// [F'F - F'E (E'E)^-1 E'F] z = F'b - F'E (E'E)^-1 E'b +// +// Now if we wish to solve Ax = b in the least squares sense, one way +// is to form this Schur complement system and solve it using +// Preconditioned Conjugate Gradients. +// +// The key operation in a conjugate gradient solver is the evaluation of the +// matrix vector product with the Schur complement +// +// S = F'F - F'E (E'E)^-1 E'F +// +// It is straightforward to see that matrix vector products with S can +// be evaluated without storing S in memory. Instead, given (E'E)^-1 +// (which for our purposes is an easily inverted block diagonal +// matrix), it can be done in terms of matrix vector products with E, +// F and (E'E)^-1. This class implements this functionality and other +// auxilliary bits needed to implement a CG solver on the Schur +// complement using the PartitionedMatrixView object. +// +// THREAD SAFETY: This class is nqot thread safe. In particular, the +// RightMultiply (and the LeftMultiply) methods are not thread safe as +// they depend on mutable arrays used for the temporaries needed to +// compute the product y += Sx; +class ImplicitSchurComplement : public LinearOperator { + public: + // num_eliminate_blocks is the number of E blocks in the matrix + // A. + // + // constant_sparsity indicates if across calls to Init, the sparsity + // structure of the matrix A remains constant or not. This makes for + // significant savings across multiple matrices A, e.g. when used in + // conjunction with an optimization algorithm. + // + // preconditioner indicates whether the inverse of the matrix F'F + // should be computed or not as a preconditioner for the Schur + // Complement. + // + // TODO(sameeragarwal): Get rid of the two bools below and replace + // them with enums. + ImplicitSchurComplement(int num_eliminate_blocks, + bool constant_sparsity, + bool preconditioner); + virtual ~ImplicitSchurComplement(); + + // Initialize the Schur complement for a linear least squares + // problem of the form + // + // |A | x = |b| + // |diag(D)| |0| + // + // If D is null, then it is treated as a zero dimensional matrix. It + // is important that the matrix A have a BlockStructure object + // associated with it and has a block structure that is compatible + // with the SchurComplement solver. + void Init(const BlockSparseMatrixBase& A, const double* D, const double* b); + + // y += Sx, where S is the Schur complement. + virtual void RightMultiply(const double* x, double* y) const; + + // The Schur complement is a symmetric positive definite matrix, + // thus the left and right multiply operators are the same. + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + + // y = (E'E)^-1 (E'b - E'F x). Given an estimate of the solution to + // the Schur complement system, this method computes the value of + // the e_block variables that were eliminated to form the Schur + // complement. + void BackSubstitute(const double* x, double* y); + + virtual int num_rows() const { return A_->num_cols_f(); } + virtual int num_cols() const { return A_->num_cols_f(); } + const Vector& rhs() const { return rhs_; } + + const BlockSparseMatrix* block_diagonal_EtE_inverse() const { + return block_diagonal_EtE_inverse_.get(); + } + + const BlockSparseMatrix* block_diagonal_FtF_inverse() const { + return block_diagonal_FtF_inverse_.get(); + } + + private: + void AddDiagonalAndInvert(const double* D, BlockSparseMatrix* matrix); + void UpdateRhs(); + + int num_eliminate_blocks_; + bool constant_sparsity_; + bool preconditioner_; + + scoped_ptr<PartitionedMatrixView> A_; + const double* D_; + const double* b_; + + scoped_ptr<BlockSparseMatrix> block_diagonal_EtE_inverse_; + scoped_ptr<BlockSparseMatrix> block_diagonal_FtF_inverse_; + + Vector rhs_; + + // Temporary storage vectors used to implement RightMultiply. + mutable Vector tmp_rows_; + mutable Vector tmp_e_cols_; + mutable Vector tmp_e_cols_2_; + mutable Vector tmp_f_cols_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/integral_types.h b/extern/libmv/third_party/ceres/internal/ceres/integral_types.h new file mode 100644 index 00000000000..01e04937e3e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/integral_types.h @@ -0,0 +1,92 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Portable typedefs for various fixed-size integers. Uses template +// metaprogramming instead of fragile compiler defines. + +#ifndef CERES_INTERNAL_INTEGRAL_TYPES_H_ +#define CERES_INTERNAL_INTEGRAL_TYPES_H_ + +namespace ceres { +namespace internal { + +// Compile time ternary on types. +template<bool kCondition, typename kTrueType, typename kFalseType> +struct Ternary { + typedef kTrueType type; +}; +template<typename kTrueType, typename kFalseType> +struct Ternary<false, kTrueType, kFalseType> { + typedef kFalseType type; +}; + +#define CERES_INTSIZE(TYPE) \ + typename Ternary<sizeof(TYPE) * 8 == kBits, TYPE, + +template<int kBits> +struct Integer { + typedef + CERES_INTSIZE(char) + CERES_INTSIZE(short) + CERES_INTSIZE(int) + CERES_INTSIZE(long int) + CERES_INTSIZE(long long) + void>::type >::type >::type >::type >::type + type; +}; + +template<int kBits> +struct UnsignedInteger { + typedef + CERES_INTSIZE(unsigned char) + CERES_INTSIZE(unsigned short) + CERES_INTSIZE(unsigned int) + CERES_INTSIZE(unsigned long int) + CERES_INTSIZE(unsigned long long) + void>::type >::type >::type >::type >::type + type; +}; + +#undef CERES_INTSIZE + +typedef Integer< 8>::type int8; +typedef Integer<16>::type int16; +typedef Integer<32>::type int32; +typedef Integer<64>::type int64; + +typedef UnsignedInteger< 8>::type uint8; +typedef UnsignedInteger<16>::type uint16; +typedef UnsignedInteger<32>::type uint32; +typedef UnsignedInteger<64>::type uint64; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_INTEGRAL_TYPES_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc new file mode 100644 index 00000000000..51303195317 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc @@ -0,0 +1,150 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/iterative_schur_complement_solver.h" + +#include <algorithm> +#include <cstring> +#include <vector> + +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/conjugate_gradients_solver.h" +#include "ceres/implicit_schur_complement.h" +#include "ceres/linear_solver.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/visibility_based_preconditioner.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_solver.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "ceres/visibility_based_preconditioner.h" + +namespace ceres { +namespace internal { + +IterativeSchurComplementSolver::IterativeSchurComplementSolver( + const LinearSolver::Options& options) + : options_(options) { +} + +IterativeSchurComplementSolver::~IterativeSchurComplementSolver() { +} + +LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + CHECK_NOTNULL(A->block_structure()); + + // Initialize a ImplicitSchurComplement object. + if ((schur_complement_ == NULL) || (!options_.constant_sparsity)) { + schur_complement_.reset( + new ImplicitSchurComplement(options_.num_eliminate_blocks, + options_.constant_sparsity, + options_.preconditioner_type == JACOBI)); + } + schur_complement_->Init(*A, per_solve_options.D, b); + + // Initialize the solution to the Schur complement system to zero. + // + // TODO(sameeragarwal): There maybe a better initialization than an + // all zeros solution. Explore other cheap starting points. + reduced_linear_system_solution_.resize(schur_complement_->num_rows()); + reduced_linear_system_solution_.setZero(); + + // Instantiate a conjugate gradient solver that runs on the Schur complement + // matrix with the block diagonal of the matrix F'F as the preconditioner. + LinearSolver::Options cg_options; + cg_options.max_num_iterations = options_.max_num_iterations; + ConjugateGradientsSolver cg_solver(cg_options); + LinearSolver::PerSolveOptions cg_per_solve_options; + + cg_per_solve_options.r_tolerance = per_solve_options.r_tolerance; + cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance; + + bool is_preconditioner_good = false; + switch (options_.preconditioner_type) { + case IDENTITY: + is_preconditioner_good = true; + break; + case JACOBI: + // We need to strip the constness of the block_diagonal_FtF_inverse + // matrix here because the only other way to initialize the struct + // cg_solve_options would be to add a constructor to it. We know + // that the only method ever called on the preconditioner is the + // RightMultiply which is a const method so we don't need to worry + // about the object getting modified. + cg_per_solve_options.preconditioner = + const_cast<BlockSparseMatrix*>( + schur_complement_->block_diagonal_FtF_inverse()); + is_preconditioner_good = true; + break; + case SCHUR_JACOBI: + case CLUSTER_JACOBI: + case CLUSTER_TRIDIAGONAL: + if (visibility_based_preconditioner_.get() == NULL) { + visibility_based_preconditioner_.reset( + new VisibilityBasedPreconditioner(*A->block_structure(), options_)); + } + is_preconditioner_good = + visibility_based_preconditioner_->Compute(*A, per_solve_options.D); + cg_per_solve_options.preconditioner = + visibility_based_preconditioner_.get(); + break; + default: + LOG(FATAL) << "Unknown Preconditioner Type"; + } + + LinearSolver::Summary cg_summary; + cg_summary.num_iterations = 0; + cg_summary.termination_type = FAILURE; + + if (is_preconditioner_good) { + cg_summary = cg_solver.Solve(schur_complement_.get(), + schur_complement_->rhs().data(), + cg_per_solve_options, + reduced_linear_system_solution_.data()); + if (cg_summary.termination_type != FAILURE) { + schur_complement_->BackSubstitute( + reduced_linear_system_solution_.data(), x); + } + } + + VLOG(2) << "CG Iterations : " << cg_summary.num_iterations; + return cg_summary; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h new file mode 100644 index 00000000000..cfeb65e1eec --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h @@ -0,0 +1,94 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ +#define CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; +class BlockSparseMatrixBase; +class ImplicitSchurComplement; +class VisibilityBasedPreconditioner; + +// This class implements an iterative solver for the linear least +// squares problems that have a bi-partitte sparsity structure common +// to Structure from Motion problems. +// +// The algorithm used by this solver was developed in a series of +// papers - "Agarwal et al, Bundle Adjustment in the Large, ECCV 2010" +// and "Wu et al, Multicore Bundle Adjustment, submitted to CVPR +// 2011" at the Univeristy of Washington. +// +// The key idea is that one can run Conjugate Gradients on the Schur +// Complement system without explicitly forming the Schur Complement +// in memory. The heavy lifting for this is done by the +// ImplicitSchurComplement class. Not forming the Schur complement in +// memory and factoring it results in substantial savings in time and +// memory. Further, iterative solvers like this open up the +// possibility of solving the Newton equations in a non-linear solver +// only approximately and terminating early, thereby saving even more +// time. +// +// For the curious, running CG on the Schur complement is the same as +// running CG on the Normal Equations with an SSOR preconditioner. For +// a proof of this fact and others related to this solver please see +// the section on Domain Decomposition Methods in Saad's book +// "Iterative Methods for Sparse Linear Systems". +class IterativeSchurComplementSolver : public BlockSparseMatrixBaseSolver { + public: + explicit IterativeSchurComplementSolver( + const LinearSolver::Options& options); + + virtual ~IterativeSchurComplementSolver(); + + private: + virtual LinearSolver::Summary SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& options, + double* x); + + LinearSolver::Options options_; + scoped_ptr<internal::ImplicitSchurComplement> schur_complement_; + scoped_ptr<VisibilityBasedPreconditioner> visibility_based_preconditioner_; + Vector reduced_linear_system_solution_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc new file mode 100644 index 00000000000..b40a5162adc --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc @@ -0,0 +1,574 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Implementation of a simple LM algorithm which uses the step sizing +// rule of "Methods for Nonlinear Least Squares" by K. Madsen, +// H.B. Nielsen and O. Tingleff. Available to download from +// +// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf +// +// The basic algorithm described in this note is an exact step +// algorithm that depends on the Newton(LM) step being solved exactly +// in each iteration. When a suitable iterative solver is available to +// solve the Newton(LM) step, the algorithm will automatically switch +// to an inexact step solution method. This trades some slowdown in +// convergence for significant savings in solve time and memory +// usage. Our implementation of the Truncated Newton algorithm follows +// the discussion and recommendataions in "Stephen G. Nash, A Survey +// of Truncated Newton Methods, Journal of Computational and Applied +// Mathematics, 124(1-2), 45-59, 2000. + +#include "ceres/levenberg_marquardt.h" + +#include <algorithm> +#include <cstdlib> +#include <cmath> +#include <cstring> +#include <string> +#include <vector> + +#include <glog/logging.h> +#include "Eigen/Core" +#include "ceres/evaluator.h" +#include "ceres/file.h" +#include "ceres/linear_least_squares_problems.h" +#include "ceres/linear_solver.h" +#include "ceres/matrix_proto.h" +#include "ceres/sparse_matrix.h" +#include "ceres/stringprintf.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { +namespace { + +// Numbers for clamping the size of the LM diagonal. The size of these +// numbers is heuristic. We will probably be adjusting them in the +// future based on more numerical experience. With jacobi scaling +// enabled, these numbers should be all but redundant. +const double kMinLevenbergMarquardtDiagonal = 1e-6; +const double kMaxLevenbergMarquardtDiagonal = 1e32; + +// Small constant for various floating point issues. +const double kEpsilon = 1e-12; + +// Number of times the linear solver should be retried in case of +// numerical failure. The retries are done by exponentially scaling up +// mu at each retry. This leads to stronger and stronger +// regularization making the linear least squares problem better +// conditioned at each retry. +const int kMaxLinearSolverRetries = 5; + +// D = 1/sqrt(diag(J^T * J)) +void EstimateScale(const SparseMatrix& jacobian, double* D) { + CHECK_NOTNULL(D); + jacobian.SquaredColumnNorm(D); + for (int i = 0; i < jacobian.num_cols(); ++i) { + D[i] = 1.0 / (kEpsilon + sqrt(D[i])); + } +} + +// D = diag(J^T * J) +void LevenbergMarquardtDiagonal(const SparseMatrix& jacobian, + double* D) { + CHECK_NOTNULL(D); + jacobian.SquaredColumnNorm(D); + for (int i = 0; i < jacobian.num_cols(); ++i) { + D[i] = min(max(D[i], kMinLevenbergMarquardtDiagonal), + kMaxLevenbergMarquardtDiagonal); + } +} + +bool RunCallback(IterationCallback* callback, + const IterationSummary& iteration_summary, + Solver::Summary* summary) { + const CallbackReturnType status = (*callback)(iteration_summary); + switch (status) { + case SOLVER_TERMINATE_SUCCESSFULLY: + summary->termination_type = USER_SUCCESS; + VLOG(1) << "Terminating on USER_SUCCESS."; + return false; + case SOLVER_ABORT: + summary->termination_type = USER_ABORT; + VLOG(1) << "Terminating on USER_ABORT."; + return false; + case SOLVER_CONTINUE: + return true; + default: + LOG(FATAL) << "Unknown status returned by callback: " + << status; + return NULL; + } +} + +} // namespace + +LevenbergMarquardt::~LevenbergMarquardt() {} + +void LevenbergMarquardt::Minimize(const Minimizer::Options& options, + Evaluator* evaluator, + LinearSolver* linear_solver, + const double* initial_parameters, + double* final_parameters, + Solver::Summary* summary) { + time_t start_time = time(NULL); + const int num_parameters = evaluator->NumParameters(); + const int num_effective_parameters = evaluator->NumEffectiveParameters(); + const int num_residuals = evaluator->NumResiduals(); + + summary->termination_type = NO_CONVERGENCE; + summary->num_successful_steps = 0; + summary->num_unsuccessful_steps = 0; + + // Allocate the various vectors needed by the algorithm. + memcpy(final_parameters, initial_parameters, + num_parameters * sizeof(*initial_parameters)); + + VectorRef x(final_parameters, num_parameters); + Vector x_new(num_parameters); + + Vector lm_step(num_effective_parameters); + Vector gradient(num_effective_parameters); + Vector scaled_gradient(num_effective_parameters); + // Jacobi scaling vector + Vector scale(num_effective_parameters); + + Vector f_model(num_residuals); + Vector f(num_residuals); + Vector f_new(num_residuals); + Vector D(num_parameters); + Vector muD(num_parameters); + + // Ask the Evaluator to create the jacobian matrix. The sparsity + // pattern of this matrix is going to remain constant, so we only do + // this once and then re-use this matrix for all subsequent Jacobian + // computations. + scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); + + double x_norm = x.norm(); + + double cost = 0.0; + D.setOnes(); + f.setZero(); + + // Do initial cost and Jacobian evaluation. + if (!evaluator->Evaluate(x.data(), &cost, f.data(), jacobian.get())) { + LOG(WARNING) << "Failed to compute residuals and Jacobian. " + << "Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + if (options.jacobi_scaling) { + EstimateScale(*jacobian, scale.data()); + jacobian->ScaleColumns(scale.data()); + } else { + scale.setOnes(); + } + + // This is a poor way to do this computation. Even if fixed_cost is + // zero, because we are subtracting two possibly large numbers, we + // are depending on exact cancellation to give us a zero here. But + // initial_cost and cost have been computed by two different + // evaluators. One which runs on the whole problem (in + // solver_impl.cc) in single threaded mode and another which runs + // here on the reduced problem, so fixed_cost can (and does) contain + // some numerical garbage with a relative magnitude of 1e-14. + // + // The right way to do this, would be to compute the fixed cost on + // just the set of residual blocks which are held constant and were + // removed from the original problem when the reduced problem was + // constructed. + summary->fixed_cost = summary->initial_cost - cost; + + double model_cost = f.squaredNorm() / 2.0; + double total_cost = summary->fixed_cost + cost; + + scaled_gradient.setZero(); + jacobian->LeftMultiply(f.data(), scaled_gradient.data()); + gradient = scaled_gradient.array() / scale.array(); + + double gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); + // We need the max here to guard againt the gradient being zero. + const double gradient_max_norm_0 = max(gradient_max_norm, kEpsilon); + double gradient_tolerance = options.gradient_tolerance * gradient_max_norm_0; + + double mu = options.tau; + double nu = 2.0; + int iteration = 0; + double actual_cost_change = 0.0; + double step_norm = 0.0; + double relative_decrease = 0.0; + + // Insane steps are steps which are not sane, i.e. there is some + // numerical kookiness going on with them. There are various reasons + // for this kookiness, some easier to diagnose then others. From the + // point of view of the non-linear solver, they are steps which + // cannot be used. We return with NUMERICAL_FAILURE after + // kMaxLinearSolverRetries consecutive insane steps. + bool step_is_sane = false; + int num_consecutive_insane_steps = 0; + + // Whether the step resulted in a sufficient decrease in the + // objective function when compared to the decrease in the value of + // the lineariztion. + bool step_is_successful = false; + + // Parse the iterations for which to dump the linear problem. + vector<int> iterations_to_dump = options.lsqp_iterations_to_dump; + sort(iterations_to_dump.begin(), iterations_to_dump.end()); + + IterationSummary iteration_summary; + iteration_summary.iteration = iteration; + iteration_summary.step_is_successful = false; + iteration_summary.cost = total_cost; + iteration_summary.cost_change = actual_cost_change; + iteration_summary.gradient_max_norm = gradient_max_norm; + iteration_summary.step_norm = step_norm; + iteration_summary.relative_decrease = relative_decrease; + iteration_summary.mu = mu; + iteration_summary.eta = options.eta; + iteration_summary.linear_solver_iterations = 0; + iteration_summary.linear_solver_time_sec = 0.0; + iteration_summary.iteration_time_sec = (time(NULL) - start_time); + if (options.logging_type >= PER_MINIMIZER_ITERATION) { + summary->iterations.push_back(iteration_summary); + } + + // Check if the starting point is an optimum. + VLOG(2) << "Gradient max norm: " << gradient_max_norm + << " tolerance: " << gradient_tolerance + << " ratio: " << gradient_max_norm / gradient_max_norm_0 + << " tolerance: " << options.gradient_tolerance; + if (gradient_max_norm <= gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating on GRADIENT_TOLERANCE. " + << "Relative gradient max norm: " + << gradient_max_norm / gradient_max_norm_0 + << " <= " << options.gradient_tolerance; + return; + } + + // Call the various callbacks. + for (int i = 0; i < options.callbacks.size(); ++i) { + if (!RunCallback(options.callbacks[i], iteration_summary, summary)) { + return; + } + } + + // We only need the LM diagonal if we are actually going to do at + // least one iteration of the optimization. So we wait to do it + // until now. + LevenbergMarquardtDiagonal(*jacobian, D.data()); + + while ((iteration < options.max_num_iterations) && + (time(NULL) - start_time) <= options.max_solver_time_sec) { + time_t iteration_start_time = time(NULL); + step_is_sane = false; + step_is_successful = false; + + IterationSummary iteration_summary; + // The while loop here is just to provide an easily breakable + // control structure. We are guaranteed to always exit this loop + // at the end of one iteration or before. + while (1) { + muD = (mu * D).array().sqrt(); + LinearSolver::PerSolveOptions solve_options; + solve_options.D = muD.data(); + solve_options.q_tolerance = options.eta; + // Disable r_tolerance checking. Since we only care about + // termination via the q_tolerance. As Nash and Sofer show, + // r_tolerance based termination is essentially useless in + // Truncated Newton methods. + solve_options.r_tolerance = -1.0; + + const time_t linear_solver_start_time = time(NULL); + LinearSolver::Summary linear_solver_summary = + linear_solver->Solve(jacobian.get(), + f.data(), + solve_options, + lm_step.data()); + iteration_summary.linear_solver_time_sec = + (time(NULL) - linear_solver_start_time); + iteration_summary.linear_solver_iterations = + linear_solver_summary.num_iterations; + + if (binary_search(iterations_to_dump.begin(), + iterations_to_dump.end(), + iteration)) { + CHECK(DumpLinearLeastSquaresProblem(options.lsqp_dump_directory, + iteration, + options.lsqp_dump_format_type, + jacobian.get(), + muD.data(), + f.data(), + lm_step.data(), + options.num_eliminate_blocks)) + << "Tried writing linear least squares problem: " + << options.lsqp_dump_directory + << " but failed."; + } + + // We ignore the case where the linear solver did not converge, + // since the partial solution computed by it still maybe of use, + // and there is no reason to ignore it, especially since we + // spent so much time computing it. + if ((linear_solver_summary.termination_type != TOLERANCE) && + (linear_solver_summary.termination_type != MAX_ITERATIONS)) { + VLOG(1) << "Linear solver failure: retrying with a higher mu"; + break; + } + + step_norm = (lm_step.array() * scale.array()).matrix().norm(); + + // Check step length based convergence. If the step length is + // too small, then we are done. + const double step_size_tolerance = options.parameter_tolerance * + (x_norm + options.parameter_tolerance); + + VLOG(2) << "Step size: " << step_norm + << " tolerance: " << step_size_tolerance + << " ratio: " << step_norm / step_size_tolerance + << " tolerance: " << options.parameter_tolerance; + if (step_norm <= options.parameter_tolerance * + (x_norm + options.parameter_tolerance)) { + summary->termination_type = PARAMETER_TOLERANCE; + VLOG(1) << "Terminating on PARAMETER_TOLERANCE." + << "Relative step size: " << step_norm / step_size_tolerance + << " <= " << options.parameter_tolerance; + return; + } + + Vector delta = -(lm_step.array() * scale.array()).matrix(); + if (!evaluator->Plus(x.data(), delta.data(), x_new.data())) { + LOG(WARNING) << "Failed to compute Plus(x, delta, x_plus_delta). " + << "Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + double cost_new = 0.0; + if (!evaluator->Evaluate(x_new.data(), &cost_new, NULL, NULL)) { + LOG(WARNING) << "Failed to compute the value of the objective " + << "function. Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + f_model.setZero(); + jacobian->RightMultiply(lm_step.data(), f_model.data()); + const double model_cost_new = + (f.segment(0, num_residuals) - f_model).squaredNorm() / 2; + + actual_cost_change = cost - cost_new; + double model_cost_change = model_cost - model_cost_new; + + VLOG(2) << "[Model cost] current: " << model_cost + << " new : " << model_cost_new + << " change: " << model_cost_change; + + VLOG(2) << "[Nonlinear cost] current: " << cost + << " new : " << cost_new + << " change: " << actual_cost_change + << " relative change: " << fabs(actual_cost_change) / cost + << " tolerance: " << options.function_tolerance; + + // In exact arithmetic model_cost_change should never be + // negative. But due to numerical precision issues, we may end up + // with a small negative number. model_cost_change which are + // negative and large in absolute value are indicative of a + // numerical failure in the solver. + if (model_cost_change < -kEpsilon) { + VLOG(1) << "Model cost change is negative.\n" + << "Current : " << model_cost + << " new : " << model_cost_new + << " change: " << model_cost_change << "\n"; + break; + } + + // If we have reached this far, then we are willing to trust the + // numerical quality of the step. + step_is_sane = true; + num_consecutive_insane_steps = 0; + + // Check function value based convergence. + if (fabs(actual_cost_change) < options.function_tolerance * cost) { + VLOG(1) << "Termination on FUNCTION_TOLERANCE." + << " Relative cost change: " << fabs(actual_cost_change) / cost + << " tolerance: " << options.function_tolerance; + summary->termination_type = FUNCTION_TOLERANCE; + return; + } + + // Clamp model_cost_change at kEpsilon from below. + if (model_cost_change < kEpsilon) { + VLOG(1) << "Clamping model cost change " << model_cost_change + << " to " << kEpsilon; + model_cost_change = kEpsilon; + } + + relative_decrease = actual_cost_change / model_cost_change; + VLOG(2) << "actual_cost_change / model_cost_change = " + << relative_decrease; + + if (relative_decrease < options.min_relative_decrease) { + VLOG(2) << "Unsuccessful step."; + break; + } + + VLOG(2) << "Successful step."; + + ++summary->num_successful_steps; + x = x_new; + x_norm = x.norm(); + + if (!evaluator->Evaluate(x.data(), &cost, f.data(), jacobian.get())) { + LOG(WARNING) << "Failed to compute residuals and jacobian. " + << "Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + if (options.jacobi_scaling) { + jacobian->ScaleColumns(scale.data()); + } + + model_cost = f.squaredNorm() / 2.0; + LevenbergMarquardtDiagonal(*jacobian, D.data()); + scaled_gradient.setZero(); + jacobian->LeftMultiply(f.data(), scaled_gradient.data()); + gradient = scaled_gradient.array() / scale.array(); + gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); + + // Check gradient based convergence. + VLOG(2) << "Gradient max norm: " << gradient_max_norm + << " tolerance: " << gradient_tolerance + << " ratio: " << gradient_max_norm / gradient_max_norm_0 + << " tolerance: " << options.gradient_tolerance; + if (gradient_max_norm <= gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating on GRADIENT_TOLERANCE. " + << "Relative gradient max norm: " + << gradient_max_norm / gradient_max_norm_0 + << " <= " << options.gradient_tolerance + << " (tolerance)."; + return; + } + + mu = mu * max(1.0 / 3.0, 1 - pow(2 * relative_decrease - 1, 3)); + nu = 2.0; + step_is_successful = true; + break; + } + + if (!step_is_sane) { + ++num_consecutive_insane_steps; + } + + if (num_consecutive_insane_steps == kMaxLinearSolverRetries) { + summary->termination_type = NUMERICAL_FAILURE; + VLOG(1) << "Too many consecutive retries; ending with numerical fail."; + + if (!options.crash_and_dump_lsqp_on_failure) { + return; + } + + // Dump debugging information to disk. + CHECK(options.lsqp_dump_format_type == TEXTFILE || + options.lsqp_dump_format_type == PROTOBUF) + << "Dumping the linear least squares problem on crash " + << "requires Solver::Options::lsqp_dump_format_type to be " + << "PROTOBUF or TEXTFILE."; + + if (DumpLinearLeastSquaresProblem(options.lsqp_dump_directory, + iteration, + options.lsqp_dump_format_type, + jacobian.get(), + muD.data(), + f.data(), + lm_step.data(), + options.num_eliminate_blocks)) { + LOG(FATAL) << "Linear least squares problem saved to: " + << options.lsqp_dump_directory + << ". Please provide this to the Ceres developers for " + << " debugging along with the v=2 log."; + } else { + LOG(FATAL) << "Tried writing linear least squares problem: " + << options.lsqp_dump_directory + << " but failed."; + } + } + + if (!step_is_successful) { + // Either the step did not lead to a decrease in cost or there + // was numerical failure. In either case we will scale mu up and + // retry. If it was a numerical failure, we hope that the + // stronger regularization will make the linear system better + // conditioned. If it was numerically sane, but there was no + // decrease in cost, then increasing mu reduces the size of the + // trust region and we look for a decrease closer to the + // linearization point. + ++summary->num_unsuccessful_steps; + mu = mu * nu; + nu = 2 * nu; + } + + ++iteration; + + total_cost = summary->fixed_cost + cost; + + iteration_summary.iteration = iteration; + iteration_summary.step_is_successful = step_is_successful; + iteration_summary.cost = total_cost; + iteration_summary.cost_change = actual_cost_change; + iteration_summary.gradient_max_norm = gradient_max_norm; + iteration_summary.step_norm = step_norm; + iteration_summary.relative_decrease = relative_decrease; + iteration_summary.mu = mu; + iteration_summary.eta = options.eta; + iteration_summary.iteration_time_sec = (time(NULL) - iteration_start_time); + + if (options.logging_type >= PER_MINIMIZER_ITERATION) { + summary->iterations.push_back(iteration_summary); + } + + // Call the various callbacks. + for (int i = 0; i < options.callbacks.size(); ++i) { + if (!RunCallback(options.callbacks[i], iteration_summary, summary)) { + return; + } + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.h b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.h new file mode 100644 index 00000000000..d00bb9095be --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.h @@ -0,0 +1,65 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Implmentation of Levenberg Marquardt algorithm based on "Methods for +// Nonlinear Least Squares" by K. Madsen, H.B. Nielsen and +// O. Tingleff. Available to download from +// +// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf +// + +#ifndef CERES_INTERNAL_LEVENBERG_MARQUARDT_H_ +#define CERES_INTERNAL_LEVENBERG_MARQUARDT_H_ + +#include "ceres/minimizer.h" +#include "ceres/solver.h" + +namespace ceres { +namespace internal { + +class Evaluator; +class LinearSolver; + +class LevenbergMarquardt : public Minimizer { + public: + virtual ~LevenbergMarquardt(); + + virtual void Minimize(const Minimizer::Options& options, + Evaluator* evaluator, + LinearSolver* linear_solver, + const double* initial_parameters, + double* final_parameters, + Solver::Summary* summary); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LEVENBERG_MARQUARDT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc new file mode 100644 index 00000000000..cca9f442fe7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc @@ -0,0 +1,744 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_least_squares_problems.h" + +#include <cstdio> +#include <string> +#include <vector> +#include <glog/logging.h> +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/file.h" +#include "ceres/matrix_proto.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/stringprintf.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) { + switch (id) { + case 0: + return LinearLeastSquaresProblem0(); + case 1: + return LinearLeastSquaresProblem1(); + case 2: + return LinearLeastSquaresProblem2(); + case 3: + return LinearLeastSquaresProblem3(); + default: + LOG(FATAL) << "Unknown problem id requested " << id; + } + return NULL; +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( + const string& filename) { + LinearLeastSquaresProblemProto problem_proto; + { + string serialized_proto; + ReadFileToStringOrDie(filename, &serialized_proto); + CHECK(problem_proto.ParseFromString(serialized_proto)); + } + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + const SparseMatrixProto& A = problem_proto.a(); + + if (A.has_block_matrix()) { + problem->A.reset(new BlockSparseMatrix(A)); + } else if (A.has_triplet_matrix()) { + problem->A.reset(new TripletSparseMatrix(A)); + } else { + problem->A.reset(new CompressedRowSparseMatrix(A)); + } + + if (problem_proto.b_size() > 0) { + problem->b.reset(new double[problem_proto.b_size()]); + for (int i = 0; i < problem_proto.b_size(); ++i) { + problem->b[i] = problem_proto.b(i); + } + } + + if (problem_proto.d_size() > 0) { + problem->D.reset(new double[problem_proto.d_size()]); + for (int i = 0; i < problem_proto.d_size(); ++i) { + problem->D[i] = problem_proto.d(i); + } + } + + if (problem_proto.d_size() > 0) { + if (problem_proto.x_size() > 0) { + problem->x_D.reset(new double[problem_proto.x_size()]); + for (int i = 0; i < problem_proto.x_size(); ++i) { + problem->x_D[i] = problem_proto.x(i); + } + } + } else { + if (problem_proto.x_size() > 0) { + problem->x.reset(new double[problem_proto.x_size()]); + for (int i = 0; i < problem_proto.x_size(); ++i) { + problem->x[i] = problem_proto.x(i); + } + } + } + + problem->num_eliminate_blocks = 0; + if (problem_proto.has_num_eliminate_blocks()) { + problem->num_eliminate_blocks = problem_proto.num_eliminate_blocks(); + } + + return problem; +} +#else +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( + const string& filename) { + LOG(FATAL) + << "Loading a least squares problem from disk requires " + << "Ceres to be built with Protocol Buffers support."; + return NULL; +} +#endif // CERES_DONT_HAVE_PROTOCOL_BUFFERS + +/* +A = [1 2] + [3 4] + [6 -10] + +b = [ 8 + 18 + -18] + +x = [2 + 3] + +D = [1 + 2] + +x_D = [1.78448275; + 2.82327586;] + */ +LinearLeastSquaresProblem* LinearLeastSquaresProblem0() { + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + TripletSparseMatrix* A = new TripletSparseMatrix(3, 2, 6); + problem->b.reset(new double[3]); + problem->D.reset(new double[2]); + + problem->x.reset(new double[2]); + problem->x_D.reset(new double[2]); + + int* Ai = A->mutable_rows(); + int* Aj = A->mutable_cols(); + double* Ax = A->mutable_values(); + + int counter = 0; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j< 2; ++j) { + Ai[counter]=i; + Aj[counter]=j; + ++counter; + } + }; + + Ax[0] = 1.; + Ax[1] = 2.; + Ax[2] = 3.; + Ax[3] = 4.; + Ax[4] = 6; + Ax[5] = -10; + A->set_num_nonzeros(6); + problem->A.reset(A); + + problem->b[0] = 8; + problem->b[1] = 18; + problem->b[2] = -18; + + problem->x[0] = 2.0; + problem->x[1] = 3.0; + + problem->D[0] = 1; + problem->D[1] = 2; + + problem->x_D[0] = 1.78448275; + problem->x_D[1] = 2.82327586; + return problem; +} + + +/* + A = [1 0 | 2 0 0 + 3 0 | 0 4 0 + 0 5 | 0 0 6 + 0 7 | 8 0 0 + 0 9 | 1 0 0 + 0 0 | 1 1 1] + + b = [0 + 1 + 2 + 3 + 4 + 5] + + c = A'* b = [ 3 + 67 + 33 + 9 + 17] + + A'A = [10 0 2 12 0 + 0 155 65 0 30 + 2 65 70 1 1 + 12 0 1 17 1 + 0 30 1 1 37] + + S = [ 42.3419 -1.4000 -11.5806 + -1.4000 2.6000 1.0000 + 11.5806 1.0000 31.1935] + + r = [ 4.3032 + 5.4000 + 5.0323] + + S\r = [ 0.2102 + 2.1367 + 0.1388] + + A\b = [-2.3061 + 0.3172 + 0.2102 + 2.1367 + 0.1388] +*/ +// The following two functions create a TripletSparseMatrix and a +// BlockSparseMatrix version of this problem. + +// TripletSparseMatrix version. +LinearLeastSquaresProblem* LinearLeastSquaresProblem1() { + int num_rows = 6; + int num_cols = 5; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + TripletSparseMatrix* A = new TripletSparseMatrix(num_rows, + num_cols, + num_rows * num_cols); + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + int* rows = A->mutable_rows(); + int* cols = A->mutable_cols(); + double* values = A->mutable_values(); + + int nnz = 0; + + // Row 1 + { + rows[nnz] = 0; + cols[nnz] = 0; + values[nnz++] = 1; + + rows[nnz] = 0; + cols[nnz] = 2; + values[nnz++] = 2; + } + + // Row 2 + { + rows[nnz] = 1; + cols[nnz] = 0; + values[nnz++] = 3; + + rows[nnz] = 1; + cols[nnz] = 3; + values[nnz++] = 4; + } + + // Row 3 + { + rows[nnz] = 2; + cols[nnz] = 1; + values[nnz++] = 5; + + rows[nnz] = 2; + cols[nnz] = 4; + values[nnz++] = 6; + } + + // Row 4 + { + rows[nnz] = 3; + cols[nnz] = 1; + values[nnz++] = 7; + + rows[nnz] = 3; + cols[nnz] = 2; + values[nnz++] = 8; + } + + // Row 5 + { + rows[nnz] = 4; + cols[nnz] = 1; + values[nnz++] = 9; + + rows[nnz] = 4; + cols[nnz] = 2; + values[nnz++] = 1; + } + + // Row 6 + { + rows[nnz] = 5; + cols[nnz] = 2; + values[nnz++] = 1; + + rows[nnz] = 5; + cols[nnz] = 3; + values[nnz++] = 1; + + rows[nnz] = 5; + cols[nnz] = 4; + values[nnz++] = 1; + } + + A->set_num_nonzeros(nnz); + CHECK(A->IsValid()); + + problem->A.reset(A); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + return problem; +} + +// BlockSparseMatrix version +LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { + int num_rows = 6; + int num_cols = 5; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + scoped_array<double> values(new double[num_rows * num_cols]); + + for (int c = 0; c < num_cols; ++c) { + bs->cols.push_back(Block()); + bs->cols.back().size = 1; + bs->cols.back().position = c; + } + + int nnz = 0; + + // Row 1 + { + values[nnz++] = 1; + values[nnz++] = 2; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 0; + row.cells.push_back(Cell(0, 0)); + row.cells.push_back(Cell(2, 1)); + } + + // Row 2 + { + values[nnz++] = 3; + values[nnz++] = 4; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 1; + row.cells.push_back(Cell(0, 2)); + row.cells.push_back(Cell(3, 3)); + } + + // Row 3 + { + values[nnz++] = 5; + values[nnz++] = 6; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 2; + row.cells.push_back(Cell(1, 4)); + row.cells.push_back(Cell(4, 5)); + } + + // Row 4 + { + values[nnz++] = 7; + values[nnz++] = 8; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 3; + row.cells.push_back(Cell(1, 6)); + row.cells.push_back(Cell(2, 7)); + } + + // Row 5 + { + values[nnz++] = 9; + values[nnz++] = 1; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 4; + row.cells.push_back(Cell(1, 8)); + row.cells.push_back(Cell(2, 9)); + } + + // Row 6 + { + values[nnz++] = 1; + values[nnz++] = 1; + values[nnz++] = 1; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 5; + row.cells.push_back(Cell(2, 10)); + row.cells.push_back(Cell(3, 11)); + row.cells.push_back(Cell(4, 12)); + } + + BlockSparseMatrix* A = new BlockSparseMatrix(bs); + memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + problem->A.reset(A); + + return problem; +} + + +/* + A = [1 0 + 3 0 + 0 5 + 0 7 + 0 9 + 0 0] + + b = [0 + 1 + 2 + 3 + 4 + 5] +*/ +// BlockSparseMatrix version +LinearLeastSquaresProblem* LinearLeastSquaresProblem3() { + int num_rows = 5; + int num_cols = 2; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + scoped_array<double> values(new double[num_rows * num_cols]); + + for (int c = 0; c < num_cols; ++c) { + bs->cols.push_back(Block()); + bs->cols.back().size = 1; + bs->cols.back().position = c; + } + + int nnz = 0; + + // Row 1 + { + values[nnz++] = 1; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 0; + row.cells.push_back(Cell(0, 0)); + } + + // Row 2 + { + values[nnz++] = 3; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 1; + row.cells.push_back(Cell(0, 1)); + } + + // Row 3 + { + values[nnz++] = 5; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 2; + row.cells.push_back(Cell(1, 2)); + } + + // Row 4 + { + values[nnz++] = 7; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 3; + row.cells.push_back(Cell(1, 3)); + } + + // Row 5 + { + values[nnz++] = 9; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 4; + row.cells.push_back(Cell(1, 4)); + } + + BlockSparseMatrix* A = new BlockSparseMatrix(bs); + memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + problem->A.reset(A); + + return problem; +} + +bool DumpLinearLeastSquaresProblemToConsole(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + CHECK_NOTNULL(A); + Matrix AA; + A->ToDenseMatrix(&AA); + LOG(INFO) << "A^T: \n" << AA.transpose(); + + if (D != NULL) { + LOG(INFO) << "A's appended diagonal:\n" + << ConstVectorRef(D, A->num_cols()); + } + + if (b != NULL) { + LOG(INFO) << "b: \n" << ConstVectorRef(b, A->num_rows()); + } + + if (x != NULL) { + LOG(INFO) << "x: \n" << ConstVectorRef(x, A->num_cols()); + } + return true; +}; + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + CHECK_NOTNULL(A); + LinearLeastSquaresProblemProto lsqp; + A->ToProto(lsqp.mutable_a()); + + if (D != NULL) { + for (int i = 0; i < A->num_cols(); ++i) { + lsqp.add_d(D[i]); + } + } + + if (b != NULL) { + for (int i = 0; i < A->num_rows(); ++i) { + lsqp.add_b(b[i]); + } + } + + if (x != NULL) { + for (int i = 0; i < A->num_cols(); ++i) { + lsqp.add_x(x[i]); + } + } + + lsqp.set_num_eliminate_blocks(num_eliminate_blocks); + string format_string = JoinPath(directory, + "lm_iteration_%03d.lsqp"); + string filename = + StringPrintf(format_string.c_str(), iteration); + LOG(INFO) << "Dumping least squares problem for iteration " << iteration + << " to disk. File: " << filename; + WriteStringToFileOrDie(lsqp.SerializeAsString(), filename); + return true; +} +#else +bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + LOG(ERROR) << "Dumping least squares problems is only " + << "supported when Ceres is compiled with " + << "protocol buffer support."; + return false; +} +#endif + +void WriteArrayToFileOrDie(const string& filename, + const double* x, + const int size) { + CHECK_NOTNULL(x); + VLOG(2) << "Writing array to: " << filename; + FILE* fptr = fopen(filename.c_str(), "w"); + CHECK_NOTNULL(fptr); + for (int i = 0; i < size; ++i) { + fprintf(fptr, "%17f\n", x[i]); + } + fclose(fptr); +} + +bool DumpLinearLeastSquaresProblemToTextFile(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + CHECK_NOTNULL(A); + string format_string = JoinPath(directory, + "lm_iteration_%03d"); + string filename_prefix = + StringPrintf(format_string.c_str(), iteration); + + { + string filename = filename_prefix + "_A.txt"; + LOG(INFO) << "writing to: " << filename; + FILE* fptr = fopen(filename.c_str(), "w"); + CHECK_NOTNULL(fptr); + A->ToTextFile(fptr); + fclose(fptr); + } + + if (D != NULL) { + string filename = filename_prefix + "_D.txt"; + WriteArrayToFileOrDie(filename, D, A->num_cols()); + } + + if (b != NULL) { + string filename = filename_prefix + "_b.txt"; + WriteArrayToFileOrDie(filename, b, A->num_rows()); + } + + if (x != NULL) { + string filename = filename_prefix + "_x.txt"; + WriteArrayToFileOrDie(filename, x, A->num_cols()); + } + + return true; +} + +bool DumpLinearLeastSquaresProblem(const string& directory, + int iteration, + DumpFormatType dump_format_type, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + switch (dump_format_type) { + case (CONSOLE): + return DumpLinearLeastSquaresProblemToConsole(directory, + iteration, + A, D, b, x, + num_eliminate_blocks); + case (PROTOBUF): + return DumpLinearLeastSquaresProblemToProtocolBuffer( + directory, + iteration, + A, D, b, x, + num_eliminate_blocks); + case (TEXTFILE): + return DumpLinearLeastSquaresProblemToTextFile(directory, + iteration, + A, D, b, x, + num_eliminate_blocks); + default: + LOG(FATAL) << "Unknown DumpFormatType " << dump_format_type; + }; + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h new file mode 100644 index 00000000000..553cc0d3db3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ +#define CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ + +#include <string> +#include <vector> +#include "ceres/sparse_matrix.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// Structure defining a linear least squares problem and if possible +// ground truth solutions. To be used by various LinearSolver tests. +struct LinearLeastSquaresProblem { + LinearLeastSquaresProblem() + : A(NULL), b(NULL), D(NULL), num_eliminate_blocks(0), + x(NULL), x_D(NULL) { + } + + scoped_ptr<SparseMatrix> A; + scoped_array<double> b; + scoped_array<double> D; + // If using the schur eliminator then how many of the variable + // blocks are e_type blocks. + int num_eliminate_blocks; + + // Solution to min_x |Ax - b|^2 + scoped_array<double> x; + // Solution to min_x |Ax - b|^2 + |Dx|^2 + scoped_array<double> x_D; +}; + +// Factories for linear least squares problem. +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id); +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( + const string& filename); + +LinearLeastSquaresProblem* LinearLeastSquaresProblem0(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem1(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem2(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem3(); + +// Write the linear least squares problem to disk. The exact format +// depends on dump_format_type. +bool DumpLinearLeastSquaresProblem(const string& directory, + int iteration, + DumpFormatType dump_format_type, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks); +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_operator.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.cc new file mode 100644 index 00000000000..4b59fa13009 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_operator.h" + +namespace ceres { +namespace internal { + +LinearOperator::~LinearOperator() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_operator.h b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.h new file mode 100644 index 00000000000..d5c15cee6a9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.h @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Base classes for access to an linear operator. + +#ifndef CERES_INTERNAL_LINEAR_OPERATOR_H_ +#define CERES_INTERNAL_LINEAR_OPERATOR_H_ + +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// This is an abstract base class for linear operators. It supports +// access to size information and left and right multiply operators. +class LinearOperator { + public: + virtual ~LinearOperator(); + + // y = y + Ax; + virtual void RightMultiply(const double* x, double* y) const = 0; + // y = y + A'x; + virtual void LeftMultiply(const double* x, double* y) const = 0; + + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_OPERATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc new file mode 100644 index 00000000000..b2e3941eea1 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_solver.h" + +#include <glog/logging.h> +#include "ceres/cgnr_solver.h" +#include "ceres/dense_qr_solver.h" +#include "ceres/iterative_schur_complement_solver.h" +#include "ceres/schur_complement_solver.h" +#include "ceres/sparse_normal_cholesky_solver.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +LinearSolver::~LinearSolver() { +} + +LinearSolver* LinearSolver::Create(const LinearSolver::Options& options) { + switch (options.type) { + case CGNR: + return new CgnrSolver(options); + + case SPARSE_NORMAL_CHOLESKY: +#ifndef CERES_NO_SUITESPARSE + return new SparseNormalCholeskySolver(options); +#else + LOG(WARNING) << "SPARSE_NORMAL_CHOLESKY is not available. Please " + << "build Ceres with SuiteSparse. Returning NULL."; + return NULL; +#endif // CERES_NO_SUITESPARSE + + case SPARSE_SCHUR: +#ifndef CERES_NO_SUITESPARSE + return new SparseSchurComplementSolver(options); +#else + LOG(WARNING) << "SPARSE_SCHUR is not available. Please " + << "build Ceres with SuiteSparse. Returning NULL."; + return NULL; +#endif // CERES_NO_SUITESPARSE + + case DENSE_SCHUR: + return new DenseSchurComplementSolver(options); + + case ITERATIVE_SCHUR: + return new IterativeSchurComplementSolver(options); + + case DENSE_QR: + return new DenseQRSolver(options); + + default: + LOG(FATAL) << "Unknown linear solver type :" + << options.type; + return NULL; // MSVC doesn't understand that LOG(FATAL) never returns. + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h new file mode 100644 index 00000000000..5860ecc8a77 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h @@ -0,0 +1,291 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Abstract interface for objects solving linear systems of various +// kinds. + +#ifndef CERES_INTERNAL_LINEAR_SOLVER_H_ +#define CERES_INTERNAL_LINEAR_SOLVER_H_ + +#include <cstddef> + +#include <glog/logging.h> +#include "ceres/block_sparse_matrix.h" +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/dense_sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class LinearOperator; + +// Abstract base class for objects that implement algorithms for +// solving linear systems +// +// Ax = b +// +// It is expected that a single instance of a LinearSolver object +// maybe used multiple times for solving different linear +// systems. This allows them to cache and reuse information across +// solves if for example the sparsity of the linear system remains +// constant. +// +// Subclasses of LinearSolver use two structs to configure themselves. +// The Options struct configures the LinearSolver object for its +// lifetime. The PerSolveOptions struct is used to specify options for +// a particular Solve call. +class LinearSolver { + public: + struct Options { + Options() + : type(SPARSE_NORMAL_CHOLESKY), + preconditioner_type(JACOBI), + min_num_iterations(1), + max_num_iterations(1), + num_threads(1), + constant_sparsity(false), + num_eliminate_blocks(0), + residual_reset_period(10), + row_block_size(Dynamic), + e_block_size(Dynamic), + f_block_size(Dynamic) { + } + + LinearSolverType type; + + PreconditionerType preconditioner_type; + + // Number of internal iterations that the solver uses. This + // parameter only makes sense for iterative solvers like CG. + int min_num_iterations; + int max_num_iterations; + + // If possible, how many threads can the solver use. + int num_threads; + + // If possible cache and reuse the symbolic factorization across + // multiple calls. + bool constant_sparsity; + + // Eliminate 0 to num_eliminate_blocks - 1 from the Normal + // equations to form a schur complement. Only used by the Schur + // complement based solver. The most common use for this parameter + // is in the case of structure from motion problems where we have + // camera blocks and point blocks. Then setting the + // num_eliminate_blocks to the number of points allows the solver + // to use the Schur complement trick. For more details see the + // description of this parameter in solver.h. + int num_eliminate_blocks; + + // Iterative solvers, e.g. Preconditioned Conjugate Gradients + // maintain a cheap estimate of the residual which may become + // inaccurate over time. Thus for non-zero values of this + // parameter, the solver can be told to recalculate the value of + // the residual using a |b - Ax| evaluation. + int residual_reset_period; + + // If the block sizes in a BlockSparseMatrix are fixed, then in + // some cases the Schur complement based solvers can detect and + // specialize on them. + // + // It is expected that these parameters are set programmatically + // rather than manually. + // + // Please see explicit_schur_complement_solver_impl.h for more + // details. + int row_block_size; + int e_block_size; + int f_block_size; + }; + + // Options for the Solve method. + struct PerSolveOptions { + PerSolveOptions() + : D(NULL), + preconditioner(NULL), + r_tolerance(0.0), + q_tolerance(0.0) { + } + + // This option only makes sense for unsymmetric linear solvers + // that can solve rectangular linear systems. + // + // Given a matrix A, an optional diagonal matrix D as a vector, + // and a vector b, the linear solver will solve for + // + // | A | x = | b | + // | D | | 0 | + // + // If D is null, then it is treated as zero, and the solver returns + // the solution to + // + // A x = b + // + // In either case, x is the vector that solves the following + // optimization problem. + // + // arg min_x ||Ax - b||^2 + ||Dx||^2 + // + // Here A is a matrix of size m x n, with full column rank. If A + // does not have full column rank, the results returned by the + // solver cannot be relied on. D, if it is not null is an array of + // size n. b is an array of size m and x is an array of size n. + double * D; + + // This option only makes sense for iterative solvers. + // + // In general the performance of an iterative linear solver + // depends on the condition number of the matrix A. For example + // the convergence rate of the conjugate gradients algorithm + // is proportional to the square root of the condition number. + // + // One particularly useful technique for improving the + // conditioning of a linear system is to precondition it. In its + // simplest form a preconditioner is a matrix M such that instead + // of solving Ax = b, we solve the linear system AM^{-1} y = b + // instead, where M is such that the condition number k(AM^{-1}) + // is smaller than the conditioner k(A). Given the solution to + // this system, x = M^{-1} y. The iterative solver takes care of + // the mechanics of solving the preconditioned system and + // returning the corrected solution x. The user only needs to + // supply a linear operator. + // + // A null preconditioner is equivalent to an identity matrix being + // used a preconditioner. + LinearOperator* preconditioner; + + + // The following tolerance related options only makes sense for + // iterative solvers. Direct solvers ignore them. + + // Solver terminates when + // + // |Ax - b| <= r_tolerance * |b|. + // + // This is the most commonly used termination criterion for + // iterative solvers. + double r_tolerance; + + // For PSD matrices A, let + // + // Q(x) = x'Ax - 2b'x + // + // be the cost of the quadratic function defined by A and b. Then, + // the solver terminates at iteration i if + // + // i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance. + // + // This termination criterion is more useful when using CG to + // solve the Newton step. This particular convergence test comes + // from Stephen Nash's work on truncated Newton + // methods. References: + // + // 1. Stephen G. Nash & Ariela Sofer, Assessing A Search + // Direction Within A Truncated Newton Method, Operation + // Research Letters 9(1990) 219-221. + // + // 2. Stephen G. Nash, A Survey of Truncated Newton Methods, + // Journal of Computational and Applied Mathematics, + // 124(1-2), 45-59, 2000. + // + double q_tolerance; + }; + + // Summary of a call to the Solve method. We should move away from + // the true/false method for determining solver success. We should + // let the summary object do the talking. + struct Summary { + Summary() + : residual_norm(0.0), + num_iterations(-1), + termination_type(FAILURE) { + } + + double residual_norm; + int num_iterations; + LinearSolverTerminationType termination_type; + }; + + virtual ~LinearSolver(); + + // Solve Ax = b. + virtual Summary Solve(LinearOperator* A, + const double* b, + const PerSolveOptions& per_solve_options, + double* x) = 0; + + static LinearSolver* Create(const Options& options); +}; + +// This templated subclass of LinearSolver serves as a base class for +// other linear solvers that depend on the particular matrix layout of +// the underlying linear operator. For example some linear solvers +// need low level access to the TripletSparseMatrix implementing the +// LinearOperator interface. This class hides those implementation +// details behind a private virtual method, and has the Solve method +// perform the necessary upcasting. +template <typename MatrixType> +class TypedLinearSolver : public LinearSolver { + public: + virtual ~TypedLinearSolver() {} + virtual LinearSolver::Summary Solve( + LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(b); + CHECK_NOTNULL(x); + return SolveImpl(down_cast<MatrixType*>(A), b, per_solve_options, x); + } + + private: + virtual LinearSolver::Summary SolveImpl( + MatrixType* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) = 0; +}; + +// Linear solvers that depend on acccess to the low level structure of +// a SparseMatrix. +typedef TypedLinearSolver<BlockSparseMatrix> BlockSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<BlockSparseMatrixBase> BlockSparseMatrixBaseSolver; // NOLINT +typedef TypedLinearSolver<CompressedRowSparseMatrix> CompressedRowSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<DenseSparseMatrix> DenseSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<TripletSparseMatrix> TripletSparseMatrixSolver; // NOLINT + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc b/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc new file mode 100644 index 00000000000..eeae74e3f95 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc @@ -0,0 +1,140 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/local_parameterization.h" +#include "ceres/rotation.h" + +namespace ceres { + +IdentityParameterization::IdentityParameterization(const int size) + : size_(size) { + CHECK_GT(size, 0); +} + +bool IdentityParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + VectorRef(x_plus_delta, size_) = + ConstVectorRef(x, size_) + ConstVectorRef(delta, size_); + return true; +} + +bool IdentityParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + MatrixRef(jacobian, size_, size_) = Matrix::Identity(size_, size_); + return true; +} + +SubsetParameterization::SubsetParameterization( + int size, + const vector<int>& constant_parameters) + : local_size_(size - constant_parameters.size()), + constancy_mask_(size, 0) { + CHECK_GT(constant_parameters.size(), 0) + << "The set of constant parameters should contain at least " + << "one element. If you do not wish to hold any parameters " + << "constant, then do not use a SubsetParameterization"; + + vector<int> constant = constant_parameters; + sort(constant.begin(), constant.end()); + CHECK(unique(constant.begin(), constant.end()) == constant.end()) + << "The set of constant parameters cannot contain duplicates"; + CHECK_LT(constant_parameters.size(), size) + << "Number of parameters held constant should be less " + << "than the size of the parameter block. If you wish " + << "to hold the entire parameter block constant, then a " + << "efficient way is to directly mark it as constant " + << "instead of using a LocalParameterization to do so."; + CHECK_GE(*min_element(constant.begin(), constant.end()), 0); + CHECK_LT(*max_element(constant.begin(), constant.end()), size); + + for (int i = 0; i < constant_parameters.size(); ++i) { + constancy_mask_[constant_parameters[i]] = 1; + } +} + +bool SubsetParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { + if (constancy_mask_[i]) { + x_plus_delta[i] = x[i]; + } else { + x_plus_delta[i] = x[i] + delta[j++]; + } + } + return true; +} + +bool SubsetParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + MatrixRef m(jacobian, constancy_mask_.size(), local_size_); + m.setZero(); + for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { + if (!constancy_mask_[i]) { + m(i, j++) = 1.0; + } + } + return true; +} + +bool QuaternionParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + const double norm_delta = + sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + if (norm_delta > 0.0) { + const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); + double q_delta[4]; + q_delta[0] = cos(norm_delta); + q_delta[1] = sin_delta_by_delta * delta[0]; + q_delta[2] = sin_delta_by_delta * delta[1]; + q_delta[3] = sin_delta_by_delta * delta[2]; + QuaternionProduct(q_delta, x, x_plus_delta); + } else { + for (int i = 0; i < 4; ++i) { + x_plus_delta[i] = x[i]; + } + } + return true; +} + +bool QuaternionParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; // NOLINT + jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; // NOLINT + jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; // NOLINT + jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; // NOLINT + return true; +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc b/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc new file mode 100644 index 00000000000..00b2b184729 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Purpose: See .h file. + +#include "ceres/loss_function.h" + +#include <cmath> +#include <cstddef> + +namespace ceres { + +void TrivialLoss::Evaluate(double s, double rho[3]) const { + rho[0] = s; + rho[1] = 1; + rho[2] = 0; +} + +void HuberLoss::Evaluate(double s, double rho[3]) const { + if (s > b_) { + // Outlier region. + // 'r' is always positive. + const double r = sqrt(s); + rho[0] = 2 * a_ * r - b_; + rho[1] = a_ / r; + rho[2] = - rho[1] / (2 * s); + } else { + // Inlier region. + rho[0] = s; + rho[1] = 1; + rho[2] = 0; + } +} + +void SoftLOneLoss::Evaluate(double s, double rho[3]) const { + const double sum = 1 + s * c_; + const double tmp = sqrt(sum); + // 'sum' and 'tmp' are always positive, assuming that 's' is. + rho[0] = 2 * b_ * (tmp - 1); + rho[1] = 1 / tmp; + rho[2] = - (c_ * rho[1]) / (2 * sum); +} + +void CauchyLoss::Evaluate(double s, double rho[3]) const { + const double sum = 1 + s * c_; + const double inv = 1 / sum; + // 'sum' and 'inv' are always positive, assuming that 's' is. + rho[0] = b_ * log(sum); + rho[1] = inv; + rho[2] = - c_ * (inv * inv); +} + +void ScaledLoss::Evaluate(double s, double rho[3]) const { + if (rho_.get() == NULL) { + rho[0] = a_ * s; + rho[1] = a_; + rho[2] = 0.0; + } else { + rho_->Evaluate(s, rho); + rho[0] *= a_; + rho[1] *= a_; + rho[2] *= a_; + } +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/map_util.h b/extern/libmv/third_party/ceres/internal/ceres/map_util.h new file mode 100644 index 00000000000..ddf1252f674 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/map_util.h @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Originally by Anton Carver + +#ifndef CERES_INTERNAL_MAP_UTIL_H_ +#define CERES_INTERNAL_MAP_UTIL_H_ + +#include <utility> +#include "ceres/internal/port.h" + +namespace ceres { + +// Perform a lookup in a map or hash_map, assuming that the key exists. +// Crash if it does not. +// +// This is intended as a replacement for operator[] as an rvalue (for reading) +// when the key is guaranteed to exist. +// +// operator[] is discouraged for several reasons: +// * It has a side-effect of inserting missing keys +// * It is not thread-safe (even when it is not inserting, it can still +// choose to resize the underlying storage) +// * It invalidates iterators (when it chooses to resize) +// * It default constructs a value object even if it doesn't need to +// +// This version assumes the key is printable, and includes it in the fatal log +// message. +template <class Collection> +const typename Collection::value_type::second_type& +FindOrDie(const Collection& collection, + const typename Collection::value_type::first_type& key) { + typename Collection::const_iterator it = collection.find(key); + CHECK(it != collection.end()) << "Map key not found: " << key; + return it->second; +} + +// Perform a lookup in a map or hash_map. +// If the key is present in the map then the value associated with that +// key is returned, otherwise the value passed as a default is returned. +template <class Collection> +const typename Collection::value_type::second_type& +FindWithDefault(const Collection& collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& value) { + typename Collection::const_iterator it = collection.find(key); + if (it == collection.end()) { + return value; + } + return it->second; +} + +// Insert a new key and value into a map or hash_map. +// If the key is not present in the map the key and value are +// inserted, otherwise nothing happens. True indicates that an insert +// took place, false indicates the key was already present. +template <class Collection> +bool InsertIfNotPresent( + Collection * const collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& value) { + pair<typename Collection::iterator, bool> ret = + collection->insert(typename Collection::value_type(key, value)); + return ret.second; +} + +// Perform a lookup in a map or hash_map. +// Same as above but the returned pointer is not const and can be used to change +// the stored value. +template <class Collection> +typename Collection::value_type::second_type* +FindOrNull(Collection& collection, // NOLINT + const typename Collection::value_type::first_type& key) { + typename Collection::iterator it = collection.find(key); + if (it == collection.end()) { + return 0; + } + return &it->second; +} + +// Test to see if a set, map, hash_set or hash_map contains a particular key. +// Returns true if the key is in the collection. +template <class Collection, class Key> +bool ContainsKey(const Collection& collection, const Key& key) { + typename Collection::const_iterator it = collection.find(key); + return it != collection.end(); +} + +// Inserts a new key/value into a map or hash_map. +// Dies if the key is already present. +template<class Collection> +void InsertOrDie(Collection* const collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& data) { + typedef typename Collection::value_type value_type; + CHECK(collection->insert(value_type(key, data)).second) + << "duplicate key: " << key; +} + +} // namespace ceres + +#endif // CERES_INTERNAL_MAP_UTIL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h b/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h new file mode 100644 index 00000000000..b8a3a1a6de6 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A portability header to make optional protocol buffer support less intrusive. + +#ifndef CERES_INTERNAL_MATRIX_PROTO_H_ +#define CERES_INTERNAL_MATRIX_PROTO_H_ + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#include "ceres/matrix.pb.h" +#endif + +#endif // CERES_INTERNAL_MATRIX_PROTO_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/minimizer.h b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h new file mode 100644 index 00000000000..77cb00cb6b4 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h @@ -0,0 +1,104 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_MINIMIZER_H_ +#define CERES_INTERNAL_MINIMIZER_H_ + +#include <vector> +#include "ceres/solver.h" +#include "ceres/iteration_callback.h" + +namespace ceres { +namespace internal { + +class Evaluator; +class LinearSolver; + +// Interface for non-linear least squares solvers. +class Minimizer { + public: + // Options struct to control the behaviour of the Minimizer. Please + // see solver.h for detailed information about the meaning and + // default values of each of these parameters. + struct Options { + explicit Options(const Solver::Options& options) { + max_num_iterations = options.max_num_iterations; + max_solver_time_sec = options.max_solver_time_sec; + gradient_tolerance = options.gradient_tolerance; + parameter_tolerance = options.parameter_tolerance; + function_tolerance = options.function_tolerance; + min_relative_decrease = options.min_relative_decrease; + eta = options.eta; + tau = options.tau; + jacobi_scaling = options.jacobi_scaling; + crash_and_dump_lsqp_on_failure = options.crash_and_dump_lsqp_on_failure; + lsqp_dump_directory = options.lsqp_dump_directory; + lsqp_iterations_to_dump = options.lsqp_iterations_to_dump; + lsqp_dump_format_type = options.lsqp_dump_format_type; + num_eliminate_blocks = options.num_eliminate_blocks; + logging_type = options.logging_type; + } + + int max_num_iterations; + int max_solver_time_sec; + double gradient_tolerance; + double parameter_tolerance; + double function_tolerance; + double min_relative_decrease; + double eta; + double tau; + bool jacobi_scaling; + bool crash_and_dump_lsqp_on_failure; + vector<int> lsqp_iterations_to_dump; + DumpFormatType lsqp_dump_format_type; + string lsqp_dump_directory; + int num_eliminate_blocks; + LoggingType logging_type; + + // List of callbacks that are executed by the Minimizer at the end + // of each iteration. + // + // Client owns these pointers. + vector<IterationCallback*> callbacks; + }; + + virtual ~Minimizer() {} + virtual void Minimize(const Options& options, + Evaluator* evaluator, + LinearSolver* linear_solver, + const double* initial_parameters, + double* final_parameters, + Solver::Summary* summary) = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_MINIMIZER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/mutex.h b/extern/libmv/third_party/ceres/internal/ceres/mutex.h new file mode 100644 index 00000000000..6514b107041 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/mutex.h @@ -0,0 +1,312 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Craig Silverstein. +// +// A simple mutex wrapper, supporting locks and read-write locks. +// You should assume the locks are *not* re-entrant. +// +// This class is meant to be internal-only and should be wrapped by an +// internal namespace. Before you use this module, please give the +// name of your internal namespace for this module. Or, if you want +// to expose it, you'll want to move it to the Google namespace. We +// cannot put this class in global namespace because there can be some +// problems when we have multiple versions of Mutex in each shared object. +// +// NOTE: by default, we have #ifdef'ed out the TryLock() method. +// This is for two reasons: +// 1) TryLock() under Windows is a bit annoying (it requires a +// #define to be defined very early). +// 2) TryLock() is broken for NO_THREADS mode, at least in NDEBUG +// mode. +// If you need TryLock(), and either these two caveats are not a +// problem for you, or you're willing to work around them, then +// feel free to #define GMUTEX_TRYLOCK, or to remove the #ifdefs +// in the code below. +// +// CYGWIN NOTE: Cygwin support for rwlock seems to be buggy: +// http://www.cygwin.com/ml/cygwin/2008-12/msg00017.html +// Because of that, we might as well use windows locks for +// cygwin. They seem to be more reliable than the cygwin pthreads layer. +// +// TRICKY IMPLEMENTATION NOTE: +// This class is designed to be safe to use during +// dynamic-initialization -- that is, by global constructors that are +// run before main() starts. The issue in this case is that +// dynamic-initialization happens in an unpredictable order, and it +// could be that someone else's dynamic initializer could call a +// function that tries to acquire this mutex -- but that all happens +// before this mutex's constructor has run. (This can happen even if +// the mutex and the function that uses the mutex are in the same .cc +// file.) Basically, because Mutex does non-trivial work in its +// constructor, it's not, in the naive implementation, safe to use +// before dynamic initialization has run on it. +// +// The solution used here is to pair the actual mutex primitive with a +// bool that is set to true when the mutex is dynamically initialized. +// (Before that it's false.) Then we modify all mutex routines to +// look at the bool, and not try to lock/unlock until the bool makes +// it to true (which happens after the Mutex constructor has run.) +// +// This works because before main() starts -- particularly, during +// dynamic initialization -- there are no threads, so a) it's ok that +// the mutex operations are a no-op, since we don't need locking then +// anyway; and b) we can be quite confident our bool won't change +// state between a call to Lock() and a call to Unlock() (that would +// require a global constructor in one translation unit to call Lock() +// and another global constructor in another translation unit to call +// Unlock() later, which is pretty perverse). +// +// That said, it's tricky, and can conceivably fail; it's safest to +// avoid trying to acquire a mutex in a global constructor, if you +// can. One way it can fail is that a really smart compiler might +// initialize the bool to true at static-initialization time (too +// early) rather than at dynamic-initialization time. To discourage +// that, we set is_safe_ to true in code (not the constructor +// colon-initializer) and set it to true via a function that always +// evaluates to true, but that the compiler can't know always +// evaluates to true. This should be good enough. + +#ifndef CERES_INTERNAL_MUTEX_H_ +#define CERES_INTERNAL_MUTEX_H_ + +#if defined(NO_THREADS) + typedef int MutexType; // to keep a lock-count +#elif defined(_WIN32) || defined(__CYGWIN32__) || defined(__CYGWIN64__) +# define WIN32_LEAN_AND_MEAN // We only need minimal includes +# ifdef GMUTEX_TRYLOCK + // We need Windows NT or later for TryEnterCriticalSection(). If you + // don't need that functionality, you can remove these _WIN32_WINNT + // lines, and change TryLock() to assert(0) or something. +# ifndef _WIN32_WINNT +# define _WIN32_WINNT 0x0400 +# endif +# endif +// To avoid macro definition of ERROR. +# define NOGDI +// To avoid macro definition of min/max. +# define NOMINMAX +# include <windows.h> + typedef CRITICAL_SECTION MutexType; +#elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK) + // Needed for pthread_rwlock_*. If it causes problems, you could take it + // out, but then you'd have to unset CERES_HAVE_RWLOCK (at least on linux -- + // it *does* cause problems for FreeBSD, or MacOSX, but isn't needed for + // locking there.) +# if defined(__linux__) && !defined(_XOPEN_SOURCE) +# define _XOPEN_SOURCE 500 // may be needed to get the rwlock calls +# endif +# include <pthread.h> + typedef pthread_rwlock_t MutexType; +#elif defined(CERES_HAVE_PTHREAD) +# include <pthread.h> + typedef pthread_mutex_t MutexType; +#else +# error Need to implement mutex.h for your architecture, or #define NO_THREADS +#endif + +// We need to include these header files after defining _XOPEN_SOURCE +// as they may define the _XOPEN_SOURCE macro. +#include <assert.h> +#include <stdlib.h> // for abort() + +namespace ceres { +namespace internal { + +class Mutex { + public: + // Create a Mutex that is not held by anybody. This constructor is + // typically used for Mutexes allocated on the heap or the stack. + // See below for a recommendation for constructing global Mutex + // objects. + inline Mutex(); + + // Destructor + inline ~Mutex(); + + inline void Lock(); // Block if needed until free then acquire exclusively + inline void Unlock(); // Release a lock acquired via Lock() +#ifdef GMUTEX_TRYLOCK + inline bool TryLock(); // If free, Lock() and return true, else return false +#endif + // Note that on systems that don't support read-write locks, these may + // be implemented as synonyms to Lock() and Unlock(). So you can use + // these for efficiency, but don't use them anyplace where being able + // to do shared reads is necessary to avoid deadlock. + inline void ReaderLock(); // Block until free or shared then acquire a share + inline void ReaderUnlock(); // Release a read share of this Mutex + inline void WriterLock() { Lock(); } // Acquire an exclusive lock + inline void WriterUnlock() { Unlock(); } // Release a lock from WriterLock() + + // TODO(hamaji): Do nothing, implement correctly. + inline void AssertHeld() {} + + private: + MutexType mutex_; + // We want to make sure that the compiler sets is_safe_ to true only + // when we tell it to, and never makes assumptions is_safe_ is + // always true. volatile is the most reliable way to do that. + volatile bool is_safe_; + + inline void SetIsSafe() { is_safe_ = true; } + + // Catch the error of writing Mutex when intending MutexLock. + Mutex(Mutex* /*ignored*/) {} + // Disallow "evil" constructors + Mutex(const Mutex&); + void operator=(const Mutex&); +}; + +// Now the implementation of Mutex for various systems +#if defined(NO_THREADS) + +// When we don't have threads, we can be either reading or writing, +// but not both. We can have lots of readers at once (in no-threads +// mode, that's most likely to happen in recursive function calls), +// but only one writer. We represent this by having mutex_ be -1 when +// writing and a number > 0 when reading (and 0 when no lock is held). +// +// In debug mode, we assert these invariants, while in non-debug mode +// we do nothing, for efficiency. That's why everything is in an +// assert. + +Mutex::Mutex() : mutex_(0) { } +Mutex::~Mutex() { assert(mutex_ == 0); } +void Mutex::Lock() { assert(--mutex_ == -1); } +void Mutex::Unlock() { assert(mutex_++ == -1); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { if (mutex_) return false; Lock(); return true; } +#endif +void Mutex::ReaderLock() { assert(++mutex_ > 0); } +void Mutex::ReaderUnlock() { assert(mutex_-- > 0); } + +#elif defined(_WIN32) || defined(__CYGWIN32__) || defined(__CYGWIN64__) + +Mutex::Mutex() { InitializeCriticalSection(&mutex_); SetIsSafe(); } +Mutex::~Mutex() { DeleteCriticalSection(&mutex_); } +void Mutex::Lock() { if (is_safe_) EnterCriticalSection(&mutex_); } +void Mutex::Unlock() { if (is_safe_) LeaveCriticalSection(&mutex_); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + TryEnterCriticalSection(&mutex_) != 0 : true; } +#endif +void Mutex::ReaderLock() { Lock(); } // we don't have read-write locks +void Mutex::ReaderUnlock() { Unlock(); } + +#elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK) + +#define SAFE_PTHREAD(fncall) do { /* run fncall if is_safe_ is true */ \ + if (is_safe_ && fncall(&mutex_) != 0) abort(); \ +} while (0) + +Mutex::Mutex() { + SetIsSafe(); + if (is_safe_ && pthread_rwlock_init(&mutex_, NULL) != 0) abort(); +} +Mutex::~Mutex() { SAFE_PTHREAD(pthread_rwlock_destroy); } +void Mutex::Lock() { SAFE_PTHREAD(pthread_rwlock_wrlock); } +void Mutex::Unlock() { SAFE_PTHREAD(pthread_rwlock_unlock); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + pthread_rwlock_trywrlock(&mutex_) == 0 : + true; } +#endif +void Mutex::ReaderLock() { SAFE_PTHREAD(pthread_rwlock_rdlock); } +void Mutex::ReaderUnlock() { SAFE_PTHREAD(pthread_rwlock_unlock); } +#undef SAFE_PTHREAD + +#elif defined(CERES_HAVE_PTHREAD) + +#define SAFE_PTHREAD(fncall) do { /* run fncall if is_safe_ is true */ \ + if (is_safe_ && fncall(&mutex_) != 0) abort(); \ +} while (0) + +Mutex::Mutex() { + SetIsSafe(); + if (is_safe_ && pthread_mutex_init(&mutex_, NULL) != 0) abort(); +} +Mutex::~Mutex() { SAFE_PTHREAD(pthread_mutex_destroy); } +void Mutex::Lock() { SAFE_PTHREAD(pthread_mutex_lock); } +void Mutex::Unlock() { SAFE_PTHREAD(pthread_mutex_unlock); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + pthread_mutex_trylock(&mutex_) == 0 : true; } +#endif +void Mutex::ReaderLock() { Lock(); } +void Mutex::ReaderUnlock() { Unlock(); } +#undef SAFE_PTHREAD + +#endif + +// -------------------------------------------------------------------------- +// Some helper classes + +// MutexLock(mu) acquires mu when constructed and releases it when destroyed. +class MutexLock { + public: + explicit MutexLock(Mutex *mu) : mu_(mu) { mu_->Lock(); } + ~MutexLock() { mu_->Unlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + MutexLock(const MutexLock&); + void operator=(const MutexLock&); +}; + +// ReaderMutexLock and WriterMutexLock do the same, for rwlocks +class ReaderMutexLock { + public: + explicit ReaderMutexLock(Mutex *mu) : mu_(mu) { mu_->ReaderLock(); } + ~ReaderMutexLock() { mu_->ReaderUnlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + ReaderMutexLock(const ReaderMutexLock&); + void operator=(const ReaderMutexLock&); +}; + +class WriterMutexLock { + public: + explicit WriterMutexLock(Mutex *mu) : mu_(mu) { mu_->WriterLock(); } + ~WriterMutexLock() { mu_->WriterUnlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + WriterMutexLock(const WriterMutexLock&); + void operator=(const WriterMutexLock&); +}; + +// Catch bug where variable name is omitted, e.g. MutexLock (&mu); +#define MutexLock(x) COMPILE_ASSERT(0, mutex_lock_decl_missing_var_name) +#define ReaderMutexLock(x) COMPILE_ASSERT(0, rmutex_lock_decl_missing_var_name) +#define WriterMutexLock(x) COMPILE_ASSERT(0, wmutex_lock_decl_missing_var_name) + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_MUTEX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc b/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc new file mode 100644 index 00000000000..f30bbc8b46b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc @@ -0,0 +1,67 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/normal_prior.h" + +#include <cstddef> +#include <vector> + +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +NormalPrior::NormalPrior(const Matrix& A, const Vector& b) + : A_(A), b_(b) { + CHECK_GT(b_.rows(), 0); + CHECK_GT(A_.rows(), 0); + CHECK_EQ(b_.rows(), A.cols()); + set_num_residuals(A_.rows()); + mutable_parameter_block_sizes()->push_back(b_.rows()); +} + +bool NormalPrior::Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + ConstVectorRef p(parameters[0], parameter_block_sizes()[0]); + VectorRef r(residuals, num_residuals()); + // The following line should read + // r = A_ * (p - b_); + // The extra eval is to get around a bug in the eigen library. + r = A_ * (p - b_).eval(); + if ((jacobians != NULL) && (jacobians[0] != NULL)) { + MatrixRef(jacobians[0], num_residuals(), parameter_block_sizes()[0]) = A_; + } + return true; +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h b/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h new file mode 100644 index 00000000000..4bac1a85828 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h @@ -0,0 +1,256 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_PARAMETER_BLOCK_H_ +#define CERES_INTERNAL_PARAMETER_BLOCK_H_ + +#include <cstdlib> +#include "ceres/integral_types.h" +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/local_parameterization.h" +#include "ceres/residual_block_utils.h" + +namespace ceres { +namespace internal { + +class ProblemImpl; + +// The parameter block encodes the location of the user's original value, and +// also the "current state" of the parameter. The evaluator uses whatever is in +// the current state of the parameter when evaluating. This is inlined since the +// methods are performance sensitive. +// +// The class is not thread-safe, unless only const methods are called. The +// parameter block may also hold a pointer to a local parameterization; the +// parameter block does not take ownership of this pointer, so the user is +// responsible for the proper disposal of the local parameterization. +class ParameterBlock { + public: + ParameterBlock(double* user_state, int size) { + Init(user_state, size, NULL); + } + ParameterBlock(double* user_state, + int size, + LocalParameterization* local_parameterization) { + Init(user_state, size, local_parameterization); + } + + // The size of the parameter block. + int Size() const { return size_; } + + // Manipulate the parameter state. + bool SetState(const double* x) { + CHECK(x != NULL) + << "Tried to set the state of constant parameter " + << "with user location " << user_state_; + CHECK(!is_constant_) + << "Tried to set the state of constant parameter " + << "with user location " << user_state_; + + state_ = x; + return UpdateLocalParameterizationJacobian(); + } + + // Copy the current parameter state out to x. This is "GetState()" rather than + // simply "state()" since it is actively copying the data into the passed + // pointer. + void GetState(double *x) const { + if (x != state_) { + memcpy(x, state_, sizeof(*state_) * size_); + } + } + + // Direct pointers to the current state. + const double* state() const { return state_; } + const double* user_state() const { return user_state_; } + double* mutable_user_state() { return user_state_; } + LocalParameterization* local_parameterization() const { + return local_parameterization_; + } + LocalParameterization* mutable_local_parameterization() { + return local_parameterization_; + } + + // Set this parameter block to vary or not. + void SetConstant() { is_constant_ = true; } + void SetVarying() { is_constant_ = false; } + bool IsConstant() const { return is_constant_; } + + // This parameter block's index in an array. + int index() const { return index_; } + void set_index(int index) { index_ = index; } + + // This parameter offset inside a larger state vector. + int state_offset() const { return state_offset_; } + void set_state_offset(int state_offset) { state_offset_ = state_offset; } + + // This parameter offset inside a larger delta vector. + int delta_offset() const { return delta_offset_; } + void set_delta_offset(int delta_offset) { delta_offset_ = delta_offset; } + + // Methods relating to the parameter block's parameterization. + + // The local to global jacobian. Returns NULL if there is no local + // parameterization for this parameter block. The returned matrix is row-major + // and has Size() rows and LocalSize() columns. + const double* LocalParameterizationJacobian() const { + return local_parameterization_jacobian_.get(); + } + + int LocalSize() const { + return (local_parameterization_ == NULL) + ? size_ + : local_parameterization_->LocalSize(); + } + + // Set the parameterization. The parameterization can be set exactly once; + // multiple calls to set the parameterization to different values will crash. + // It is an error to pass NULL for the parameterization. The parameter block + // does not take ownership of the parameterization. + void SetParameterization(LocalParameterization* new_parameterization) { + CHECK(new_parameterization != NULL) << "NULL parameterization invalid."; + CHECK(new_parameterization->GlobalSize() == size_) + << "Invalid parameterization for parameter block. The parameter block " + << "has size " << size_ << " while the parameterization has a global " + << "size of " << new_parameterization->GlobalSize() << ". Did you " + << "accidentally use the wrong parameter block or parameterization?"; + if (new_parameterization != local_parameterization_) { + CHECK(local_parameterization_ == NULL) + << "Can't re-set the local parameterization; it leads to " + << "ambiguous ownership."; + local_parameterization_ = new_parameterization; + local_parameterization_jacobian_.reset( + new double[local_parameterization_->GlobalSize() * + local_parameterization_->LocalSize()]); + CHECK(UpdateLocalParameterizationJacobian()) + "Local parameterization Jacobian computation failed" + "for x: " << ConstVectorRef(state_, Size()).transpose(); + } else { + // Ignore the case that the parameterizations match. + } + } + + // Generalization of the addition operation. This is the same as + // LocalParameterization::Plus() but uses the parameter's current state + // instead of operating on a passed in pointer. + bool Plus(const double *x, const double* delta, double* x_plus_delta) { + if (local_parameterization_ == NULL) { + VectorRef(x_plus_delta, size_) = ConstVectorRef(x, size_) + + ConstVectorRef(delta, size_); + return true; + } + return local_parameterization_->Plus(x, delta, x_plus_delta); + } + + private: + void Init(double* user_state, + int size, + LocalParameterization* local_parameterization) { + user_state_ = user_state; + size_ = size; + is_constant_ = false; + state_ = user_state_; + + local_parameterization_ = NULL; + if (local_parameterization != NULL) { + SetParameterization(local_parameterization); + } + + index_ = -1; + state_offset_ = -1; + delta_offset_ = -1; + } + + bool UpdateLocalParameterizationJacobian() { + if (local_parameterization_ == NULL) { + return true; + } + + // Update the local to global Jacobian. In some cases this is + // wasted effort; if this is a bottleneck, we will find a solution + // at that time. + + const int jacobian_size = Size() * LocalSize(); + InvalidateArray(jacobian_size, + local_parameterization_jacobian_.get()); + if (!local_parameterization_->ComputeJacobian( + state_, + local_parameterization_jacobian_.get())) { + LOG(WARNING) << "Local parameterization Jacobian computation failed" + "for x: " << ConstVectorRef(state_, Size()).transpose(); + return false; + } + + if (!IsArrayValid(jacobian_size, local_parameterization_jacobian_.get())) { + LOG(WARNING) << "Local parameterization Jacobian computation returned" + << "an invalid matrix for x: " + << ConstVectorRef(state_, Size()).transpose() + << "\n Jacobian matrix : " + << ConstMatrixRef(local_parameterization_jacobian_.get(), + Size(), + LocalSize()); + return false; + } + return true; + } + + double* user_state_; + int size_; + bool is_constant_; + LocalParameterization* local_parameterization_; + + // The "state" of the parameter. These fields are only needed while the + // solver is running. While at first glance using mutable is a bad idea, this + // ends up simplifying the internals of Ceres enough to justify the potential + // pitfalls of using "mutable." + mutable const double* state_; + mutable scoped_array<double> local_parameterization_jacobian_; + + // The index of the parameter. This is used by various other parts of Ceres to + // permit switching from a ParameterBlock* to an index in another array. + int32 index_; + + // The offset of this parameter block inside a larger state vector. + int32 state_offset_; + + // The offset of this parameter block inside a larger delta vector. + int32 delta_offset_; + + // Necessary so ProblemImpl can clean up the parameterizations. + friend class ProblemImpl; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PARAMETER_BLOCK_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc new file mode 100644 index 00000000000..fcf8fd53aed --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc @@ -0,0 +1,315 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10 + +#include "ceres/partitioned_matrix_view.h" + +#include <algorithm> +#include <cstring> +#include <vector> +#include <glog/logging.h> +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +PartitionedMatrixView::PartitionedMatrixView( + const BlockSparseMatrixBase& matrix, + int num_col_blocks_a) + : matrix_(matrix), + num_col_blocks_e_(num_col_blocks_a) { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + CHECK_NOTNULL(bs); + + num_col_blocks_f_ = bs->cols.size() - num_col_blocks_a; + + // Compute the number of row blocks in E. The number of row blocks + // in E maybe less than the number of row blocks in the input matrix + // as some of the row blocks at the bottom may not have any + // e_blocks. For a definition of what an e_block is, please see + // explicit_schur_complement_solver.h + num_row_blocks_e_ = 0; + for (int r = 0; r < bs->rows.size(); ++r) { + const vector<Cell>& cells = bs->rows[r].cells; + if (cells[0].block_id < num_col_blocks_a) { + ++num_row_blocks_e_; + } + } + + // Compute the number of columns in E and F. + num_cols_e_ = 0; + num_cols_f_ = 0; + + for (int c = 0; c < bs->cols.size(); ++c) { + const Block& block = bs->cols[c]; + if (c < num_col_blocks_a) { + num_cols_e_ += block.size; + } else { + num_cols_f_ += block.size; + } + } + + CHECK_EQ(num_cols_e_ + num_cols_f_, matrix_.num_cols()); +} + +PartitionedMatrixView::~PartitionedMatrixView() { +} + +// The next four methods don't seem to be particularly cache +// friendly. This is an artifact of how the BlockStructure of the +// input matrix is constructed. These methods will benefit from +// multithreading as well as improved data layout. + +void PartitionedMatrixView::RightMultiplyE(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over the first num_row_blocks_e_ row blocks, and multiply + // by the first cell in each row block. + for (int r = 0; r < num_row_blocks_e_; ++r) { + const double* row_values = matrix_.RowBlockValues(r); + const Cell& cell = bs->rows[r].cells[0]; + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + const int col_block_id = cell.block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + ConstVectorRef xref(x + col_block_pos, col_block_size); + VectorRef yref(y + row_block_pos, row_block_size); + ConstMatrixRef m(row_values + cell.position, + row_block_size, + col_block_size); + yref += m.lazyProduct(xref); + } +} + +void PartitionedMatrixView::RightMultiplyF(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over row blocks, and if the row block is in E, then + // multiply by all the cells except the first one which is of type + // E. If the row block is not in E (i.e its in the bottom + // num_row_blocks - num_row_blocks_e row blocks), then all the cells + // are of type F and multiply by them all. + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + VectorRef yref(y + row_block_pos, row_block_size); + const vector<Cell>& cells = bs->rows[r].cells; + for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { + const double* row_values = matrix_.RowBlockValues(r); + const int col_block_id = cells[c].block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + ConstVectorRef xref(x + col_block_pos - num_cols_e(), + col_block_size); + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + yref += m.lazyProduct(xref); + } + } +} + +void PartitionedMatrixView::LeftMultiplyE(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over the first num_row_blocks_e_ row blocks, and multiply + // by the first cell in each row block. + for (int r = 0; r < num_row_blocks_e_; ++r) { + const Cell& cell = bs->rows[r].cells[0]; + const double* row_values = matrix_.RowBlockValues(r); + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + const int col_block_id = cell.block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + ConstVectorRef xref(x + row_block_pos, row_block_size); + VectorRef yref(y + col_block_pos, col_block_size); + ConstMatrixRef m(row_values + cell.position, + row_block_size, + col_block_size); + yref += m.transpose().lazyProduct(xref); + } +} + +void PartitionedMatrixView::LeftMultiplyF(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over row blocks, and if the row block is in E, then + // multiply by all the cells except the first one which is of type + // E. If the row block is not in E (i.e its in the bottom + // num_row_blocks - num_row_blocks_e row blocks), then all the cells + // are of type F and multiply by them all. + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + ConstVectorRef xref(x + row_block_pos, row_block_size); + const vector<Cell>& cells = bs->rows[r].cells; + for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { + const double* row_values = matrix_.RowBlockValues(r); + const int col_block_id = cells[c].block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + VectorRef yref(y + col_block_pos - num_cols_e(), col_block_size); + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + yref += m.transpose().lazyProduct(xref); + } + } +} + +// Given a range of columns blocks of a matrix m, compute the block +// structure of the block diagonal of the matrix m(:, +// start_col_block:end_col_block)'m(:, start_col_block:end_col_block) +// and return a BlockSparseMatrix with the this block structure. The +// caller owns the result. +BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalMatrixLayout( + int start_col_block, int end_col_block) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + CompressedRowBlockStructure* block_diagonal_structure = + new CompressedRowBlockStructure; + + int block_position = 0; + int diagonal_cell_position = 0; + + // Iterate over the column blocks, creating a new diagonal block for + // each column block. + for (int c = start_col_block; c < end_col_block; ++c) { + const Block& block = bs->cols[c]; + block_diagonal_structure->cols.push_back(Block()); + Block& diagonal_block = block_diagonal_structure->cols.back(); + diagonal_block.size = block.size; + diagonal_block.position = block_position; + + block_diagonal_structure->rows.push_back(CompressedRow()); + CompressedRow& row = block_diagonal_structure->rows.back(); + row.block = diagonal_block; + + row.cells.push_back(Cell()); + Cell& cell = row.cells.back(); + cell.block_id = c - start_col_block; + cell.position = diagonal_cell_position; + + block_position += block.size; + diagonal_cell_position += block.size * block.size; + } + + // Build a BlockSparseMatrix with the just computed block + // structure. + return new BlockSparseMatrix(block_diagonal_structure); +} + +BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalEtE() const { + BlockSparseMatrix* block_diagonal = + CreateBlockDiagonalMatrixLayout(0, num_col_blocks_e_); + UpdateBlockDiagonalEtE(block_diagonal); + return block_diagonal; +} + +BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalFtF() const { + BlockSparseMatrix* block_diagonal = + CreateBlockDiagonalMatrixLayout( + num_col_blocks_e_, num_col_blocks_e_ + num_col_blocks_f_); + UpdateBlockDiagonalFtF(block_diagonal); + return block_diagonal; +} + +// Similar to the code in RightMultiplyE, except instead of the matrix +// vector multiply its an outer product. +// +// block_diagonal = block_diagonal(E'E) +void PartitionedMatrixView::UpdateBlockDiagonalEtE( + BlockSparseMatrix* block_diagonal) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + + block_diagonal->SetZero(); + + for (int r = 0; r < num_row_blocks_e_ ; ++r) { + const double* row_values = matrix_.RowBlockValues(r); + const Cell& cell = bs->rows[r].cells[0]; + const int row_block_size = bs->rows[r].block.size; + const int block_id = cell.block_id; + const int col_block_size = bs->cols[block_id].size; + ConstMatrixRef m(row_values + cell.position, + row_block_size, + col_block_size); + + const int cell_position = + block_diagonal_structure->rows[block_id].cells[0].position; + + MatrixRef(block_diagonal->mutable_values() + cell_position, + col_block_size, col_block_size).noalias() += m.transpose() * m; + } +} + +// Similar to the code in RightMultiplyF, except instead of the matrix +// vector multiply its an outer product. +// +// block_diagonal = block_diagonal(F'F) +// +void PartitionedMatrixView::UpdateBlockDiagonalFtF( + BlockSparseMatrix* block_diagonal) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + + block_diagonal->SetZero(); + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_size = bs->rows[r].block.size; + const vector<Cell>& cells = bs->rows[r].cells; + const double* row_values = matrix_.RowBlockValues(r); + for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { + const int col_block_id = cells[c].block_id; + const int col_block_size = bs->cols[col_block_id].size; + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + const int diagonal_block_id = col_block_id - num_col_blocks_e_; + const int cell_position = + block_diagonal_structure->rows[diagonal_block_id].cells[0].position; + + MatrixRef(block_diagonal->mutable_values() + cell_position, + col_block_size, col_block_size).noalias() += m.transpose() * m; + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h new file mode 100644 index 00000000000..cfe4de5b436 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h @@ -0,0 +1,121 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// For generalized bi-partite Jacobian matrices that arise in +// Structure from Motion related problems, it is sometimes useful to +// have access to the two parts of the matrix as linear operators +// themselves. This class provides that functionality. + +#ifndef CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ +#define CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ + +#include "ceres/block_sparse_matrix.h" + +namespace ceres { +namespace internal { + +// Given generalized bi-partite matrix A = [E F], with the same block +// structure as required by the Schur complement based solver, found +// in explicit_schur_complement_solver.h, provide access to the +// matrices E and F and their outer products E'E and F'F with +// themselves. +// +// Lack of BlockStructure object will result in a crash and if the +// block structure of the matrix does not satisfy the requirements of +// the Schur complement solver it will result in unpredictable and +// wrong output. +// +// This class lives in the internal name space as its a utility class +// to be used by the IterativeSchurComplementSolver class, found in +// iterative_schur_complement_solver.h, and is not meant for general +// consumption. +class PartitionedMatrixView { + public: + // matrix = [E F], where the matrix E contains the first + // num_col_blocks_a column blocks. + PartitionedMatrixView(const BlockSparseMatrixBase& matrix, + int num_col_blocks_a); + ~PartitionedMatrixView(); + + // y += E'x + void LeftMultiplyE(const double* x, double* y) const; + + // y += F'x + void LeftMultiplyF(const double* x, double* y) const; + + // y += Ex + void RightMultiplyE(const double* x, double* y) const; + + // y += Fx + void RightMultiplyF(const double* x, double* y) const; + + // Create and return the block diagonal of the matrix E'E. + BlockSparseMatrix* CreateBlockDiagonalEtE() const; + + // Create and return the block diagonal of the matrix F'F. + BlockSparseMatrix* CreateBlockDiagonalFtF() const; + + // Compute the block diagonal of the matrix E'E and store it in + // block_diagonal. The matrix block_diagonal is expected to have a + // BlockStructure (preferably created using + // CreateBlockDiagonalMatrixEtE) which is has the same structure as + // the block diagonal of E'E. + void UpdateBlockDiagonalEtE(BlockSparseMatrix* block_diagonal) const; + + // Compute the block diagonal of the matrix F'F and store it in + // block_diagonal. The matrix block_diagonal is expected to have a + // BlockStructure (preferably created using + // CreateBlockDiagonalMatrixFtF) which is has the same structure as + // the block diagonal of F'F. + void UpdateBlockDiagonalFtF(BlockSparseMatrix* block_diagonal) const; + + int num_col_blocks_e() const { return num_col_blocks_e_; } + int num_col_blocks_f() const { return num_col_blocks_f_; } + int num_cols_e() const { return num_cols_e_; } + int num_cols_f() const { return num_cols_f_; } + int num_rows() const { return matrix_.num_rows(); } + int num_cols() const { return matrix_.num_cols(); } + + private: + BlockSparseMatrix* CreateBlockDiagonalMatrixLayout(int start_col_block, + int end_col_block) const; + + const BlockSparseMatrixBase& matrix_; + int num_row_blocks_e_; + int num_col_blocks_e_; + int num_col_blocks_f_; + int num_cols_e_; + int num_cols_f_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem.cc b/extern/libmv/third_party/ceres/internal/ceres/problem.cc new file mode 100644 index 00000000000..b8c25d9db84 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/problem.cc @@ -0,0 +1,149 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#include "ceres/problem.h" + +#include <vector> +#include "ceres/problem_impl.h" + +namespace ceres { + +class ResidualBlock; + +Problem::Problem() : problem_impl_(new internal::ProblemImpl) {} +Problem::Problem(const Problem::Options& options) + : problem_impl_(new internal::ProblemImpl(options)) {} +Problem::~Problem() {} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + parameter_blocks); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3, x4); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3, x4, x5); +} + +void Problem::AddParameterBlock(double* values, int size) { + problem_impl_->AddParameterBlock(values, size); +} + +void Problem::AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization) { + problem_impl_->AddParameterBlock(values, size, local_parameterization); +} + +void Problem::SetParameterBlockConstant(double* values) { + problem_impl_->SetParameterBlockConstant(values); +} + +void Problem::SetParameterBlockVariable(double* values) { + problem_impl_->SetParameterBlockVariable(values); +} + +void Problem::SetParameterization( + double* values, + LocalParameterization* local_parameterization) { + problem_impl_->SetParameterization(values, local_parameterization); +} + +int Problem::NumParameterBlocks() const { + return problem_impl_->NumParameterBlocks(); +} + +int Problem::NumParameters() const { + return problem_impl_->NumParameters(); +} + +int Problem::NumResidualBlocks() const { + return problem_impl_->NumResidualBlocks(); +} + +int Problem::NumResiduals() const { + return problem_impl_->NumResiduals(); +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc new file mode 100644 index 00000000000..68242477d6f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc @@ -0,0 +1,359 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#include "ceres/problem_impl.h" + +#include <algorithm> +#include <cstddef> +#include <set> +#include <string> +#include <utility> +#include <vector> + +#include <glog/logging.h> +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/stl_util.h" +#include "ceres/map_util.h" +#include "ceres/stringprintf.h" +#include "ceres/cost_function.h" +#include "ceres/loss_function.h" + +namespace ceres { +namespace internal { + +typedef map<double*, internal::ParameterBlock*> ParameterMap; + +// Returns true if two regions of memory, a and b, with sizes size_a and size_b +// respectively, overlap. +static bool RegionsAlias(const double* a, int size_a, + const double* b, int size_b) { + return (a < b) ? b < (a + size_a) + : a < (b + size_b); +} + +static void CheckForNoAliasing(double* existing_block, + int existing_block_size, + double* new_block, + int new_block_size) { + CHECK(!RegionsAlias(existing_block, existing_block_size, + new_block, new_block_size)) + << "Aliasing detected between existing parameter block at memory " + << "location " << existing_block + << " and has size " << existing_block_size << " with new parameter " + << "block that has memory adderss " << new_block << " and would have " + << "size " << new_block_size << "."; +} + +static ParameterBlock* InternalAddParameterBlock( + double* values, + int size, + ParameterMap* parameter_map, + vector<ParameterBlock*>* parameter_blocks) { + CHECK(values) << "Null pointer passed to AddParameterBlock for a parameter " + << "with size " << size; + + // Ignore the request if there is a block for the given pointer already. + ParameterMap::iterator it = parameter_map->find(values); + if (it != parameter_map->end()) { + int existing_size = it->second->Size(); + CHECK(size == existing_size) + << "Tried adding a parameter block with the same double pointer, " + << values << ", twice, but with different block sizes. Original " + << "size was " << existing_size << " but new size is " + << size; + return it->second; + } + // Before adding the parameter block, also check that it doesn't alias any + // other parameter blocks. + if (!parameter_map->empty()) { + ParameterMap::iterator lb = parameter_map->lower_bound(values); + + // If lb is not the first block, check the previous block for aliasing. + if (lb != parameter_map->begin()) { + ParameterMap::iterator previous = lb; + --previous; + CheckForNoAliasing(previous->first, + previous->second->Size(), + values, + size); + } + + // If lb is not off the end, check lb for aliasing. + if (lb != parameter_map->end()) { + CheckForNoAliasing(lb->first, + lb->second->Size(), + values, + size); + } + } + ParameterBlock* new_parameter_block = new ParameterBlock(values, size); + (*parameter_map)[values] = new_parameter_block; + parameter_blocks->push_back(new_parameter_block); + return new_parameter_block; +} + +ProblemImpl::ProblemImpl() : program_(new internal::Program) {} +ProblemImpl::ProblemImpl(const Problem::Options& options) + : options_(options), + program_(new internal::Program) {} + +ProblemImpl::~ProblemImpl() { + // Collect the unique cost/loss functions and delete the residuals. + set<CostFunction*> cost_functions; + set<LossFunction*> loss_functions; + for (int i = 0; i < program_->residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = program_->residual_blocks_[i]; + + // The const casts here are legit, since ResidualBlock holds these + // pointers as const pointers but we have ownership of them and + // have the right to destroy them when the destructor is called. + if (options_.cost_function_ownership == TAKE_OWNERSHIP) { + cost_functions.insert( + const_cast<CostFunction*>(residual_block->cost_function())); + } + if (options_.loss_function_ownership == TAKE_OWNERSHIP) { + loss_functions.insert( + const_cast<LossFunction*>(residual_block->loss_function())); + } + + delete residual_block; + } + + // Collect the unique parameterizations and delete the parameters. + set<LocalParameterization*> local_parameterizations; + for (int i = 0; i < program_->parameter_blocks_.size(); ++i) { + ParameterBlock* parameter_block = program_->parameter_blocks_[i]; + + if (options_.local_parameterization_ownership == TAKE_OWNERSHIP) { + local_parameterizations.insert(parameter_block->local_parameterization_); + } + + delete parameter_block; + } + + // Delete the owned cost/loss functions and parameterizations. + STLDeleteContainerPointers(local_parameterizations.begin(), + local_parameterizations.end()); + STLDeleteContainerPointers(cost_functions.begin(), + cost_functions.end()); + STLDeleteContainerPointers(loss_functions.begin(), + loss_functions.end()); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks) { + CHECK_NOTNULL(cost_function); + CHECK_EQ(parameter_blocks.size(), + cost_function->parameter_block_sizes().size()); + + // Check the sizes match. + const vector<int16>& parameter_block_sizes = + cost_function->parameter_block_sizes(); + CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size()) + << "Number of blocks input is different than the number of blocks " + << "that the cost function expects."; + + // Check for duplicate parameter blocks. + vector<double*> sorted_parameter_blocks(parameter_blocks); + sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); + vector<double*>::const_iterator duplicate_items = + unique(sorted_parameter_blocks.begin(), + sorted_parameter_blocks.end()); + if (duplicate_items != sorted_parameter_blocks.end()) { + string blocks; + for (int i = 0; i < parameter_blocks.size(); ++i) { + blocks += internal::StringPrintf(" %p ", parameter_blocks[i]); + } + + LOG(FATAL) << "Duplicate parameter blocks in a residual parameter " + << "are not allowed. Parameter block pointers: [" + << blocks << "]"; + } + + // Add parameter blocks and convert the double*'s to parameter blocks. + vector<ParameterBlock*> parameter_block_ptrs(parameter_blocks.size()); + for (int i = 0; i < parameter_blocks.size(); ++i) { + parameter_block_ptrs[i] = + InternalAddParameterBlock(parameter_blocks[i], + parameter_block_sizes[i], + ¶meter_block_map_, + &program_->parameter_blocks_); + } + + // Check that the block sizes match the block sizes expected by the + // cost_function. + for (int i = 0; i < parameter_block_ptrs.size(); ++i) { + CHECK_EQ(cost_function->parameter_block_sizes()[i], + parameter_block_ptrs[i]->Size()) + << "The cost function expects parameter block " << i + << " of size " << cost_function->parameter_block_sizes()[i] + << " but was given a block of size " + << parameter_block_ptrs[i]->Size(); + } + + ResidualBlock* new_residual_block = + new ResidualBlock(cost_function, + loss_function, + parameter_block_ptrs); + program_->residual_blocks_.push_back(new_residual_block); + return new_residual_block; +} + +// Unfortunately, macros don't help much to reduce this code, and var args don't +// work because of the ambiguous case that there is no loss function. +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + residual_parameters.push_back(x5); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + + +void ProblemImpl::AddParameterBlock(double* values, int size) { + InternalAddParameterBlock(values, + size, + ¶meter_block_map_, + &program_->parameter_blocks_); +} + +void ProblemImpl::AddParameterBlock( + double* values, + int size, + LocalParameterization* local_parameterization) { + ParameterBlock* parameter_block = + InternalAddParameterBlock(values, + size, + ¶meter_block_map_, + &program_->parameter_blocks_); + if (local_parameterization != NULL) { + parameter_block->SetParameterization(local_parameterization); + } +} + +void ProblemImpl::SetParameterBlockConstant(double* values) { + FindOrDie(parameter_block_map_, values)->SetConstant(); +} + +void ProblemImpl::SetParameterBlockVariable(double* values) { + FindOrDie(parameter_block_map_, values)->SetVarying(); +} + +void ProblemImpl::SetParameterization( + double* values, + LocalParameterization* local_parameterization) { + FindOrDie(parameter_block_map_, values) + ->SetParameterization(local_parameterization); +} + +int ProblemImpl::NumParameterBlocks() const { + return program_->NumParameterBlocks(); +} + +int ProblemImpl::NumParameters() const { + return program_->NumParameters(); +} + +int ProblemImpl::NumResidualBlocks() const { + return program_->NumResidualBlocks(); +} + +int ProblemImpl::NumResiduals() const { + return program_->NumResiduals(); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h new file mode 100644 index 00000000000..523860e652a --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h @@ -0,0 +1,127 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// This is the implementation of the public Problem API. The pointer to +// implementation (PIMPL) idiom makes it possible for Ceres internal code to +// refer to the private data members without needing to exposing it to the +// world. An alternative to PIMPL is to have a factory which returns instances +// of a virtual base class; while that approach would work, it requires clients +// to always put a Problem object into a scoped pointer; this needlessly muddies +// client code for little benefit. Therefore, the PIMPL comprise was chosen. + +#ifndef CERES_PUBLIC_PROBLEM_IMPL_H_ +#define CERES_PUBLIC_PROBLEM_IMPL_H_ + +#include <map> +#include <vector> + +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/problem.h" +#include "ceres/types.h" + +namespace ceres { + +class CostFunction; +class LossFunction; +class LocalParameterization; + +namespace internal { + +class Program; +class ResidualBlock; + +class ProblemImpl { + public: + typedef map<double*, ParameterBlock*> ParameterMap; + + ProblemImpl(); + explicit ProblemImpl(const Problem::Options& options); + + ~ProblemImpl(); + + // See the public problem.h file for description of these methods. + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5); + void AddParameterBlock(double* values, int size); + void AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization); + void SetParameterBlockConstant(double* values); + void SetParameterBlockVariable(double* values); + void SetParameterization(double* values, + LocalParameterization* local_parameterization); + int NumParameterBlocks() const; + int NumParameters() const; + int NumResidualBlocks() const; + int NumResiduals() const; + + const Program& program() const { return *program_; } + Program* mutable_program() { return program_.get(); } + + const ParameterMap& parameter_map() const { return parameter_block_map_; } + + private: + const Problem::Options options_; + + // The mapping from user pointers to parameter blocks. + map<double*, ParameterBlock*> parameter_block_map_; + + internal::scoped_ptr<internal::Program> program_; + DISALLOW_COPY_AND_ASSIGN(ProblemImpl); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_PROBLEM_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/program.cc b/extern/libmv/third_party/ceres/internal/ceres/program.cc new file mode 100644 index 00000000000..444b1020253 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/program.cc @@ -0,0 +1,233 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/program.h" + +#include <map> +#include <vector> +#include "ceres/parameter_block.h" +#include "ceres/residual_block.h" +#include "ceres/stl_util.h" +#include "ceres/map_util.h" +#include "ceres/problem.h" +#include "ceres/cost_function.h" +#include "ceres/loss_function.h" +#include "ceres/local_parameterization.h" + +namespace ceres { +namespace internal { + +Program::Program() {} + +Program::Program(const Program& program) + : parameter_blocks_(program.parameter_blocks_), + residual_blocks_(program.residual_blocks_) { +} + +const vector<ParameterBlock*>& Program::parameter_blocks() const { + return parameter_blocks_; +} + +const vector<ResidualBlock*>& Program::residual_blocks() const { + return residual_blocks_; +} + +vector<ParameterBlock*>* Program::mutable_parameter_blocks() { + return ¶meter_blocks_; +} + +vector<ResidualBlock*>* Program::mutable_residual_blocks() { + return &residual_blocks_; +} + +bool Program::StateVectorToParameterBlocks(const double *state) { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + if (!parameter_blocks_[i]->SetState(state)) { + return false; + } + state += parameter_blocks_[i]->Size(); + } + return true; +} + +void Program::ParameterBlocksToStateVector(double *state) const { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->GetState(state); + state += parameter_blocks_[i]->Size(); + } +} + +void Program::CopyParameterBlockStateToUserState() { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->GetState( + parameter_blocks_[i]->mutable_user_state()); + } +} + +bool Program::Plus(const double* state, + const double* delta, + double* state_plus_delta) const { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) { + return false; + } + state += parameter_blocks_[i]->Size(); + delta += parameter_blocks_[i]->LocalSize(); + state_plus_delta += parameter_blocks_[i]->Size(); + } + return true; +} + +void Program::SetParameterOffsetsAndIndex() { + // Set positions for all parameters appearing as arguments to residuals to one + // past the end of the parameter block array. + for (int i = 0; i < residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = residual_blocks_[i]; + for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { + residual_block->parameter_blocks()[j]->set_index(-1); + } + } + // For parameters that appear in the program, set their position and offset. + int state_offset = 0; + int delta_offset = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->set_index(i); + parameter_blocks_[i]->set_state_offset(state_offset); + parameter_blocks_[i]->set_delta_offset(delta_offset); + state_offset += parameter_blocks_[i]->Size(); + delta_offset += parameter_blocks_[i]->LocalSize(); + } +} + +int Program::NumResidualBlocks() const { + return residual_blocks_.size(); +} + +int Program::NumParameterBlocks() const { + return parameter_blocks_.size(); +} + +int Program::NumResiduals() const { + int num_residuals = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + num_residuals += residual_blocks_[i]->NumResiduals(); + } + return num_residuals; +} + +int Program::NumParameters() const { + int num_parameters = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + num_parameters += parameter_blocks_[i]->Size(); + } + return num_parameters; +} + +int Program::NumEffectiveParameters() const { + int num_parameters = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + num_parameters += parameter_blocks_[i]->LocalSize(); + } + return num_parameters; +} + +int Program::MaxScratchDoublesNeededForEvaluate() const { + // Compute the scratch space needed for evaluate. + int max_scratch_bytes_for_evaluate = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + max_scratch_bytes_for_evaluate = + max(max_scratch_bytes_for_evaluate, + residual_blocks_[i]->NumScratchDoublesForEvaluate()); + } + return max_scratch_bytes_for_evaluate; +} + +int Program::MaxDerivativesPerResidualBlock() const { + int max_derivatives = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + int derivatives = 0; + ResidualBlock* residual_block = residual_blocks_[i]; + int num_parameters = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameters; ++j) { + derivatives += residual_block->NumResiduals() * + residual_block->parameter_blocks()[j]->LocalSize(); + } + max_derivatives = max(max_derivatives, derivatives); + } + return max_derivatives; +} + +int Program::MaxParametersPerResidualBlock() const { + int max_parameters = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + max_parameters = max(max_parameters, + residual_blocks_[i]->NumParameterBlocks()); + } + return max_parameters; +} + +bool Program::Evaluate(double* cost, double* residuals) { + *cost = 0.0; + + // Scratch space is only needed if residuals is NULL. + scoped_array<double> scratch; + if (residuals == NULL) { + scratch.reset(new double[MaxScratchDoublesNeededForEvaluate()]); + } else { + // TODO(keir): Is this needed? Check by removing the equivalent statement in + // dense_evaluator.cc and running the tests. + VectorRef(residuals, NumResiduals()).setZero(); + } + + for (int i = 0; i < residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = residual_blocks_[i]; + + // Evaluate the cost function for this residual. + double residual_cost; + if (!residual_block->Evaluate(&residual_cost, + residuals, + NULL, // No jacobian. + scratch.get())) { + return false; + } + + // Accumulate residual cost into the total cost. + *cost += residual_cost; + + // Update the residuals cursor. + if (residuals != NULL) { + residuals += residual_block->NumResiduals(); + } + } + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/program.h b/extern/libmv/third_party/ceres/internal/ceres/program.h new file mode 100644 index 00000000000..113d352d562 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/program.h @@ -0,0 +1,128 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_PROGRAM_H_ +#define CERES_INTERNAL_PROGRAM_H_ + +#include <vector> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class ParameterBlock; +class ProblemImpl; +class ResidualBlock; + +// A nonlinear least squares optimization problem. This is different from the +// similarly-named "Problem" object, which offers a mutation interface for +// adding and modifying parameters and residuals. The Program contains the core +// part of the Problem, which is the parameters and the residuals, stored in a +// particular ordering. The ordering is critical, since it defines the mapping +// between (residual, parameter) pairs and a position in the jacobian of the +// objective function. Various parts of Ceres transform one Program into +// another; for example, the first stage of solving involves stripping all +// constant parameters and residuals. This is in contrast with Problem, which is +// not built for transformation. +class Program { + public: + Program(); + explicit Program(const Program& program); + + // The ordered parameter and residual blocks for the program. + const vector<ParameterBlock*>& parameter_blocks() const; + const vector<ResidualBlock*>& residual_blocks() const; + vector<ParameterBlock*>* mutable_parameter_blocks(); + vector<ResidualBlock*>* mutable_residual_blocks(); + + // Serialize to/from the program and update states. + // + // NOTE: Setting the state of a parameter block can trigger the + // computation of the Jacobian of its local parameterization. If + // this computation fails for some reason, then this method returns + // false and the state of the parameter blocks cannot be trusted. + bool StateVectorToParameterBlocks(const double *state); + void ParameterBlocksToStateVector(double *state) const; + + // Copy internal state out to the user's parameters. + void CopyParameterBlockStateToUserState(); + + // Update a state vector for the program given a delta. + bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const; + + // Set the parameter indices and offsets. This permits mapping backward + // from a ParameterBlock* to an index in the parameter_blocks() vector. For + // any parameter block p, after calling SetParameterOffsetsAndIndex(), it + // is true that + // + // parameter_blocks()[p->index()] == p + // + // If a parameter appears in a residual but not in the parameter block, then + // it will have an index of -1. + // + // This also updates p->state_offset() and p->delta_offset(), which are the + // position of the parameter in the state and delta vector respectively. + void SetParameterOffsetsAndIndex(); + + // See problem.h for what these do. + int NumParameterBlocks() const; + int NumParameters() const; + int NumEffectiveParameters() const; + int NumResidualBlocks() const; + int NumResiduals() const; + + int MaxScratchDoublesNeededForEvaluate() const; + int MaxDerivativesPerResidualBlock() const; + int MaxParametersPerResidualBlock() const; + + // Evaluate the cost and maybe the residuals for the program. If residuals is + // NULL, then residuals are not calculated. If the jacobian is needed, instead + // use the various evaluators (e.g. dense_evaluator.h). + // + // This is a trivial implementation of evaluate not intended for use in the + // core solving loop. The other evaluators, which support constructing the + // jacobian in addition to the cost and residuals, are considerably + // complicated by the need to construct the jacobian. + bool Evaluate(double* cost, double* residuals); + + private: + // The Program does not own the ParameterBlock or ResidualBlock objects. + vector<ParameterBlock*> parameter_blocks_; + vector<ResidualBlock*> residual_blocks_; + + friend class ProblemImpl; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PROGRAM_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h b/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h new file mode 100644 index 00000000000..7ec74b1b269 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h @@ -0,0 +1,279 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// The ProgramEvaluator runs the cost functions contained in each residual block +// and stores the result into a jacobian. The particular type of jacobian is +// abstracted out using two template parameters: +// +// - An "EvaluatePreparer" that is responsible for creating the array with +// pointers to the jacobian blocks where the cost function evaluates to. +// - A "JacobianWriter" that is responsible for storing the resulting +// jacobian blocks in the passed sparse matrix. +// +// This abstraction affords an efficient evaluator implementation while still +// supporting writing to multiple sparse matrix formats. For example, when the +// ProgramEvaluator is parameterized for writing to block sparse matrices, the +// residual jacobians are written directly into their final position in the +// block sparse matrix by the user's CostFunction; there is no copying. +// +// The evaluation is threaded with OpenMP. +// +// The EvaluatePreparer and JacobianWriter interfaces are as follows: +// +// class EvaluatePreparer { +// // Prepare the jacobians array for use as the destination of a call to +// // a cost function's evaluate method. +// void Prepare(const ResidualBlock* residual_block, +// int residual_block_index, +// SparseMatrix* jacobian, +// double** jacobians); +// } +// +// class JacobianWriter { +// // Create a jacobian that this writer can write. Same as +// // Evaluator::CreateJacobian. +// SparseMatrix* CreateJacobian() const; +// +// // Create num_threads evaluate preparers. Caller owns result which must +// // be freed with delete[]. Resulting preparers are valid while *this is. +// EvaluatePreparer* CreateEvaluatePreparers(int num_threads); +// +// // Write the block jacobians from a residual block evaluation to the +// // larger sparse jacobian. +// void Write(int residual_id, +// int residual_offset, +// double** jacobians, +// SparseMatrix* jacobian); +// } +// +// Note: The ProgramEvaluator is not thread safe, since internally it maintains +// some per-thread scratch space. + +#ifndef CERES_INTERNAL_PROGRAM_EVALUATOR_H_ +#define CERES_INTERNAL_PROGRAM_EVALUATOR_H_ + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +template<typename EvaluatePreparer, typename JacobianWriter> +class ProgramEvaluator : public Evaluator { + public: + ProgramEvaluator(const Evaluator::Options &options, Program* program) + : options_(options), + program_(program), + jacobian_writer_(options, program), + evaluate_preparers_( + jacobian_writer_.CreateEvaluatePreparers(options.num_threads)) { +#ifndef CERES_USE_OPENMP + CHECK_EQ(1, options_.num_threads) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads=1 is supported."; +#endif + + BuildResidualLayout(*program, &residual_layout_); + evaluate_scratch_.reset(CreateEvaluatorScratch(*program, + options.num_threads)); + } + + // Implementation of Evaluator interface. + SparseMatrix* CreateJacobian() const { + return jacobian_writer_.CreateJacobian(); + } + + bool Evaluate(const double* state, + double* cost, + double* residuals, + SparseMatrix* jacobian) { + // The parameters are stateful, so set the state before evaluating. + if (!program_->StateVectorToParameterBlocks(state)) { + return false; + } + + if (jacobian) { + jacobian->SetZero(); + } + + // Each thread gets it's own cost and evaluate scratch space. + for (int i = 0; i < options_.num_threads; ++i) { + evaluate_scratch_[i].cost = 0.0; + } + + // This bool is used to disable the loop if an error is encountered + // without breaking out of it. The remaining loop iterations are still run, + // but with an empty body, and so will finish quickly. + bool abort = false; + int num_residual_blocks = program_->NumResidualBlocks(); +#pragma omp parallel for num_threads(options_.num_threads) + for (int i = 0; i < num_residual_blocks; ++i) { +// Disable the loop instead of breaking, as required by OpenMP. +#pragma omp flush(abort) + if (abort) { + continue; + } + +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + EvaluatePreparer* preparer = &evaluate_preparers_[thread_id]; + EvaluateScratch* scratch = &evaluate_scratch_[thread_id]; + + // Prepare block residuals if requested. + const ResidualBlock* residual_block = program_->residual_blocks()[i]; + double* block_residuals = (residuals != NULL) + ? (residuals + residual_layout_[i]) + : NULL; + + // Prepare block jacobians if requested. + double** block_jacobians = NULL; + if (jacobian != NULL) { + preparer->Prepare(residual_block, + i, + jacobian, + scratch->jacobian_block_ptrs.get()); + block_jacobians = scratch->jacobian_block_ptrs.get(); + } + + // Evaluate the cost, residuals, and jacobians. + double block_cost; + if (!residual_block->Evaluate(&block_cost, + block_residuals, + block_jacobians, + scratch->scratch.get())) { + abort = true; +// This ensures that the OpenMP threads have a consistent view of 'abort'. Do +// the flush inside the failure case so that there is usually only one +// synchronization point per loop iteration instead of two. +#pragma omp flush(abort) + continue; + } + + scratch->cost += block_cost; + + if (jacobian != NULL) { + jacobian_writer_.Write(i, + residual_layout_[i], + block_jacobians, + jacobian); + } + } + + if (!abort) { + // Sum the cost from each thread. + (*cost) = 0.0; + for (int i = 0; i < options_.num_threads; ++i) { + (*cost) += evaluate_scratch_[i].cost; + } + } + return !abort; + } + + bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const { + return program_->Plus(state, delta, state_plus_delta); + } + + int NumParameters() const { + return program_->NumParameters(); + } + int NumEffectiveParameters() const { + return program_->NumEffectiveParameters(); + } + + int NumResiduals() const { + return program_->NumResiduals(); + } + + private: + struct EvaluateScratch { + void Init(int max_parameters_per_residual_block, + int max_scratch_doubles_needed_for_evaluate) { + jacobian_block_ptrs.reset( + new double*[max_parameters_per_residual_block]); + scratch.reset(new double[max_scratch_doubles_needed_for_evaluate]); + } + + double cost; + scoped_array<double> scratch; + scoped_array<double*> jacobian_block_ptrs; + }; + + static void BuildResidualLayout(const Program& program, + vector<int>* residual_layout) { + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + residual_layout->resize(program.NumResidualBlocks()); + int residual_pos = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + const int num_residuals = residual_blocks[i]->NumResiduals(); + (*residual_layout)[i] = residual_pos; + residual_pos += num_residuals; + } + } + + // Create scratch space for each thread evaluating the program. + static EvaluateScratch* CreateEvaluatorScratch(const Program& program, + int num_threads) { + int max_parameters_per_residual_block = + program.MaxParametersPerResidualBlock(); + int max_scratch_doubles_needed_for_evaluate = + program.MaxScratchDoublesNeededForEvaluate(); + + EvaluateScratch* evaluate_scratch = new EvaluateScratch[num_threads]; + for (int i = 0; i < num_threads; i++) { + evaluate_scratch[i].Init(max_parameters_per_residual_block, + max_scratch_doubles_needed_for_evaluate); + } + return evaluate_scratch; + } + + Evaluator::Options options_; + Program* program_; + JacobianWriter jacobian_writer_; + scoped_array<EvaluatePreparer> evaluate_preparers_; + scoped_array<EvaluateScratch> evaluate_scratch_; + vector<int> residual_layout_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PROGRAM_EVALUATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/random.h b/extern/libmv/third_party/ceres/internal/ceres/random.h new file mode 100644 index 00000000000..769e0b4dd27 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/random.h @@ -0,0 +1,47 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_RANDOM_H_ +#define CERES_INTERNAL_RANDOM_H_ + +namespace ceres { + +inline double RandDouble() { + double r = rand(); + return r / RAND_MAX; +} + +inline int Uniform(int n) { + return rand() % n; +} + +} // namespace ceres + +#endif // CERES_INTERNAL_RANDOM_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc b/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc new file mode 100644 index 00000000000..03867891dba --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc @@ -0,0 +1,212 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/residual_block.h" + +#include <algorithm> +#include <cstddef> +#include <vector> + +#include "ceres/corrector.h" +#include "ceres/parameter_block.h" +#include "ceres/residual_block_utils.h" +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/local_parameterization.h" +#include "ceres/loss_function.h" + +namespace ceres { +namespace internal { + +ResidualBlock::ResidualBlock(const CostFunction* cost_function, + const LossFunction* loss_function, + const vector<ParameterBlock*>& parameter_blocks) + : cost_function_(cost_function), + loss_function_(loss_function), + parameter_blocks_( + new ParameterBlock* [ + cost_function->parameter_block_sizes().size()]) { + std::copy(parameter_blocks.begin(), + parameter_blocks.end(), + parameter_blocks_.get()); +} + +bool ResidualBlock::Evaluate(double* cost, + double* residuals, + double** jacobians, + double* scratch) const { + const int num_parameter_blocks = NumParameterBlocks(); + const int num_residuals = cost_function_->num_residuals(); + + // Collect the parameters from their blocks. This will rarely allocate, since + // residuals taking more than 8 parameter block arguments are rare. + FixedArray<const double*, 8> parameters(num_parameter_blocks); + for (int i = 0; i < num_parameter_blocks; ++i) { + parameters[i] = parameter_blocks_[i]->state(); + } + + // Put pointers into the scratch space into global_jacobians as appropriate. + FixedArray<double*, 8> global_jacobians(num_parameter_blocks); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + if (jacobians[i] != NULL && + parameter_block->LocalParameterizationJacobian() != NULL) { + global_jacobians[i] = scratch; + scratch += num_residuals * parameter_block->Size(); + } else { + global_jacobians[i] = jacobians[i]; + } + } + } + + // If the caller didn't request residuals, use the scratch space for them. + bool outputting_residuals = (residuals != NULL); + if (!outputting_residuals) { + residuals = scratch; + } + + // Invalidate the evaluation buffers so that we can check them after + // the CostFunction::Evaluate call, to see if all the return values + // that were required were written to and that they are finite. + double** eval_jacobians = (jacobians != NULL) ? global_jacobians.get() : NULL; + + InvalidateEvaluation(*this, cost, residuals, eval_jacobians); + + if (!cost_function_->Evaluate(parameters.get(), residuals, eval_jacobians) || + !IsEvaluationValid(*this, + parameters.get(), + cost, + residuals, + eval_jacobians)) { + string message = + "\n\n" + "Error in evaluating the ResidualBlock.\n\n" + "There are two possible reasons. Either the CostFunction did not evaluate and fill all \n" // NOLINT + "residual and jacobians that were requested or there was a non-finite value (nan/infinite)\n" // NOLINT + "generated during the or jacobian computation. \n\n" + + EvaluationToString(*this, + parameters.get(), + cost, + residuals, + eval_jacobians); + LOG(WARNING) << message; + return false; + } + + double squared_norm = VectorRef(residuals, num_residuals).squaredNorm(); + + // Update the jacobians with the local parameterizations. + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + if (jacobians[i] != NULL) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + + // Apply local reparameterization to the jacobians. + if (parameter_block->LocalParameterizationJacobian() != NULL) { + ConstMatrixRef local_to_global( + parameter_block->LocalParameterizationJacobian(), + parameter_block->Size(), + parameter_block->LocalSize()); + MatrixRef global_jacobian(global_jacobians[i], + num_residuals, + parameter_block->Size()); + MatrixRef local_jacobian(jacobians[i], + num_residuals, + parameter_block->LocalSize()); + local_jacobian.noalias() = global_jacobian * local_to_global; + } + } + } + } + + if (loss_function_ == NULL) { + *cost = 0.5 * squared_norm; + return true; + } + + double rho[3]; + loss_function_->Evaluate(squared_norm, rho); + *cost = 0.5 * rho[0]; + + // No jacobians and not outputting residuals? All done. Doing an early exit + // here avoids constructing the "Corrector" object below in a common case. + if (jacobians == NULL && !outputting_residuals) { + return true; + } + + // Correct for the effects of the loss function. The jacobians need to be + // corrected before the residuals, since they use the uncorrected residuals. + Corrector correct(squared_norm, rho); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + if (jacobians[i] != NULL) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + + // Correct the jacobians for the loss function. + correct.CorrectJacobian(num_residuals, + parameter_block->LocalSize(), + residuals, + jacobians[i]); + } + } + } + + // Correct the residuals with the loss function. + if (outputting_residuals) { + correct.CorrectResiduals(num_residuals, residuals); + } + return true; +} + +int ResidualBlock::NumScratchDoublesForEvaluate() const { + // Compute the amount of scratch space needed to store the full-sized + // jacobians. For parameters that have no local parameterization no storage + // is needed and the passed-in jacobian array is used directly. Also include + // space to store the residuals, which is needed for cost-only evaluations. + // This is slightly pessimistic, since both won't be needed all the time, but + // the amount of excess should not cause problems for the caller. + int num_parameters = NumParameterBlocks(); + int scratch_doubles = 1; + for (int i = 0; i < num_parameters; ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + if (!parameter_block->IsConstant() && + parameter_block->LocalParameterizationJacobian() != NULL) { + scratch_doubles += parameter_block->Size(); + } + } + scratch_doubles *= NumResiduals(); + return scratch_doubles; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block.h b/extern/libmv/third_party/ceres/internal/ceres/residual_block.h new file mode 100644 index 00000000000..e0a06e78958 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block.h @@ -0,0 +1,124 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) +// +// Purpose : Class and struct definitions for parameter and residual blocks. + +#ifndef CERES_INTERNAL_RESIDUAL_BLOCK_H_ +#define CERES_INTERNAL_RESIDUAL_BLOCK_H_ + +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +class LossFunction; + +namespace internal { + +class ParameterBlock; + +// A term in the least squares problem. The mathematical form of each term in +// the overall least-squares cost function is: +// +// 1 +// --- loss_function( || cost_function(block1, block2, ...) ||^2 ), +// 2 +// +// Storing the cost function and the loss function separately permits optimizing +// the problem with standard non-linear least techniques, without requiring a +// more general non-linear solver. +// +// The residual block stores pointers to but does not own the cost functions, +// loss functions, and parameter blocks. +class ResidualBlock { + public: + ResidualBlock(const CostFunction* cost_function, + const LossFunction* loss_function, + const vector<ParameterBlock*>& parameter_blocks); + + // Evaluates the residual term, storing the scalar cost in *cost, the residual + // components in *residuals, and the jacobians between the parameters and + // residuals in jacobians[i], in row-major order. If residuals is NULL, the + // residuals are not computed. If jacobians is NULL, no jacobians are + // computed. If jacobians[i] is NULL, then the jacobian for that parameter is + // not computed. + // + // Evaluate needs scratch space which must be supplied by the caller via + // scratch. The array should have at least NumScratchDoublesForEvaluate() + // space available. + // + // The return value indicates the success or failure. If the function returns + // false, the caller should expect the the output memory locations to have + // been modified. + // + // The returned cost and jacobians have had robustification and local + // parameterizations applied already; for example, the jacobian for a + // 4-dimensional quaternion parameter using the "QuaternionParameterization" + // is num_residuals by 3 instead of num_residuals by 4. + bool Evaluate(double* cost, + double* residuals, + double** jacobians, + double* scratch) const; + + const CostFunction* cost_function() const { return cost_function_; } + const LossFunction* loss_function() const { return loss_function_; } + + // Access the parameter blocks for this residual. The array has size + // NumParameterBlocks(). + ParameterBlock* const* parameter_blocks() const { + return parameter_blocks_.get(); + } + + // Number of variable blocks that this residual term depends on. + int NumParameterBlocks() const { + return cost_function_->parameter_block_sizes().size(); + } + + // The size of the residual vector returned by this residual function. + int NumResiduals() const { return cost_function_->num_residuals(); } + + // The minimum amount of scratch space needed to pass to Evaluate(). + int NumScratchDoublesForEvaluate() const; + + private: + const CostFunction* cost_function_; + const LossFunction* loss_function_; + scoped_array<ParameterBlock*> parameter_blocks_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_RESIDUAL_BLOCK_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.cc b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.cc new file mode 100644 index 00000000000..28e03130844 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.cc @@ -0,0 +1,185 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/residual_block_utils.h" + +#include <cmath> +#include <cstddef> +#include <limits> +#include <glog/logging.h> +#include "ceres/residual_block.h" +#include "ceres/parameter_block.h" +#include "ceres/stringprintf.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" + +#ifdef _MSC_VER +# define isfinite _finite +#endif + +namespace ceres { +namespace internal { + +// It is a near impossibility that user code generates this exact +// value in normal operation, thus we will use it to fill arrays +// before passing them to user code. If on return an element of the +// array still contains this value, we will assume that the user code +// did not write to that memory location. +static const double kImpossibleValue = 1e302; + +bool IsArrayValid(const int size, const double* x) { + if (x != NULL) { + for (int i = 0; i < size; ++i) { + if (!isfinite(x[i]) || (x[i] == kImpossibleValue)) { + return false; + } + } + } + return true; +} + +void InvalidateArray(const int size, double* x) { + if (x != NULL) { + for (int i = 0; i < size; ++i) { + x[i] = kImpossibleValue; + } + } +} + +void InvalidateEvaluation(const ResidualBlock& block, + double* cost, + double* residuals, + double** jacobians) { + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + + InvalidateArray(1, cost); + InvalidateArray(num_residuals, residuals); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + InvalidateArray(num_residuals * parameter_block_size, jacobians[i]); + } + } +} + +// Utility routine to print an array of doubles to a string. If the +// array pointer is NULL, it is treated as an array of zeros. +void AppendArrayToString(const int size, const double* x, string* result) { + for (int i = 0; i < size; ++i) { + if (x == NULL) { + StringAppendF(result, "Not Computed "); + } else { + if (x[i] == kImpossibleValue) { + StringAppendF(result, "Uninitialized "); + } else { + StringAppendF(result, "%12g ", x[i]); + } + } + } +} + +string EvaluationToString(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians) { + CHECK_NOTNULL(cost); + CHECK_NOTNULL(residuals); + + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + string result = ""; + + StringAppendF(&result, + "Residual Block size: %d parameter blocks x %d residuals\n\n", + num_parameter_blocks, num_residuals); + result += + "For each parameter block, the value of the parameters are printed in the first column \n" // NOLINT + "and the value of the jacobian under the corresponding residual. If a ParameterBlock was \n" // NOLINT + "held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry \n" // NOLINT + "of the Jacobian/residual array was requested but was not written to by user code, it is \n" // NOLINT + "indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating \n" // NOLINT + "to Inf or NaN is also an error. \n\n"; // NOLINT + + string space = "Residuals: "; + result += space; + AppendArrayToString(num_residuals, residuals, &result); + StringAppendF(&result, "\n\n"); + + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + StringAppendF( + &result, "Parameter Block %d, size: %d\n", i, parameter_block_size); + StringAppendF(&result, "\n"); + for (int j = 0; j < parameter_block_size; ++j) { + AppendArrayToString(1, parameters[i] + j, &result); + StringAppendF(&result, "| "); + for (int k = 0; k < num_residuals; ++k) { + AppendArrayToString(1, + (jacobians != NULL && jacobians[i] != NULL) + ? jacobians[i] + k * parameter_block_size + j + : NULL, + &result); + } + StringAppendF(&result, "\n"); + } + StringAppendF(&result, "\n"); + } + StringAppendF(&result, "\n"); + return result; +} + +bool IsEvaluationValid(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians) { + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + + if (!IsArrayValid(num_residuals, residuals)) { + return false; + } + + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + if (!IsArrayValid(num_residuals * parameter_block_size, jacobians[i])) { + return false; + } + } + } + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.h b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.h new file mode 100644 index 00000000000..228867cc60c --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.h @@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Utility routines for ResidualBlock evaluation. +// +// These are useful for detecting two common class of errors. +// +// 1. Uninitialized memory - where the user for some reason did not +// compute part of a cost/residual/jacobian. +// +// 2. Numerical failure while computing the cost/residual/jacobian, +// e.g. NaN, infinities etc. This is particularly useful since the +// automatic differentiation code does computations that are not +// evident to the user and can silently generate hard to debug errors. + +#ifndef CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ +#define CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ + +#include <string> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class ResidualBlock; + +// Fill the array x with an impossible value that the user code is +// never expected to compute. +void InvalidateArray(int size, double* x); + +// Check if all the entries of the array x are valid, i.e. all the +// values in the array should be finite and none of them should be +// equal to the "impossible" value used by InvalidateArray. +bool IsArrayValid(int size, const double* x); + +// Invalidate cost, resdual and jacobian arrays (if not NULL). +void InvalidateEvaluation(const ResidualBlock& block, + double* cost, + double* residuals, + double** jacobians); + +// Check if any of the arrays cost, residuals or jacobians contains an +// NaN, return true if it does. +bool IsEvaluationValid(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians); + +// Create a string representation of the Residual block containing the +// value of the parameters, residuals and jacobians if present. +// Useful for debugging output. +string EvaluationToString(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.cc b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.cc new file mode 100644 index 00000000000..ac6d8aa279a --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.cc @@ -0,0 +1,218 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Based on the templated version in public/numeric_diff_cost_function.h. + +#include "ceres/runtime_numeric_diff_cost_function.h" + +#include <algorithm> +#include <numeric> +#include <vector> + +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/cost_function.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { +namespace { + +bool EvaluateJacobianForParameterBlock(const CostFunction* function, + int parameter_block_size, + int parameter_block, + RuntimeNumericDiffMethod method, + double relative_step_size, + double const* residuals_at_eval_point, + double** parameters, + double** jacobians) { + using Eigen::Map; + using Eigen::Matrix; + using Eigen::Dynamic; + using Eigen::RowMajor; + + typedef Matrix<double, Dynamic, 1> ResidualVector; + typedef Matrix<double, Dynamic, 1> ParameterVector; + typedef Matrix<double, Dynamic, Dynamic, RowMajor> JacobianMatrix; + + int num_residuals = function->num_residuals(); + + Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block], + num_residuals, + parameter_block_size); + + // Mutate one element at a time and then restore. + Map<ParameterVector> x_plus_delta(parameters[parameter_block], + parameter_block_size); + ParameterVector x(x_plus_delta); + ParameterVector step_size = x.array().abs() * relative_step_size; + + // To handle cases where a paremeter is exactly zero, instead use the mean + // step_size for the other dimensions. + double fallback_step_size = step_size.sum() / step_size.rows(); + if (fallback_step_size == 0.0) { + // If all the parameters are zero, there's no good answer. Use the given + // relative step_size as absolute step_size and hope for the best. + fallback_step_size = relative_step_size; + } + + // For each parameter in the parameter block, use finite differences to + // compute the derivative for that parameter. + for (int j = 0; j < parameter_block_size; ++j) { + if (step_size(j) == 0.0) { + // The parameter is exactly zero, so compromise and use the mean step_size + // from the other parameters. This can break in many cases, but it's hard + // to pick a good number without problem specific knowledge. + step_size(j) = fallback_step_size; + } + x_plus_delta(j) = x(j) + step_size(j); + + ResidualVector residuals(num_residuals); + if (!function->Evaluate(parameters, &residuals[0], NULL)) { + // Something went wrong; bail. + return false; + } + + // Compute this column of the jacobian in 3 steps: + // 1. Store residuals for the forward part. + // 2. Subtract residuals for the backward (or 0) part. + // 3. Divide out the run. + parameter_jacobian.col(j) = residuals; + + double one_over_h = 1 / step_size(j); + if (method == CENTRAL) { + // Compute the function on the other side of x(j). + x_plus_delta(j) = x(j) - step_size(j); + + if (!function->Evaluate(parameters, &residuals[0], NULL)) { + // Something went wrong; bail. + return false; + } + parameter_jacobian.col(j) -= residuals; + one_over_h /= 2; + } else { + // Forward difference only; reuse existing residuals evaluation. + parameter_jacobian.col(j) -= + Map<const ResidualVector>(residuals_at_eval_point, num_residuals); + } + x_plus_delta(j) = x(j); // Restore x_plus_delta. + + // Divide out the run to get slope. + parameter_jacobian.col(j) *= one_over_h; + } + return true; +} + +class RuntimeNumericDiffCostFunction : public CostFunction { + public: + RuntimeNumericDiffCostFunction(const CostFunction* function, + RuntimeNumericDiffMethod method, + double relative_step_size) + : function_(function), + method_(method), + relative_step_size_(relative_step_size) { + *mutable_parameter_block_sizes() = function->parameter_block_sizes(); + set_num_residuals(function->num_residuals()); + } + + virtual ~RuntimeNumericDiffCostFunction() { } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + // Get the function value (residuals) at the the point to evaluate. + bool success = function_->Evaluate(parameters, residuals, NULL); + if (!success) { + // Something went wrong; ignore the jacobian. + return false; + } + if (!jacobians) { + // Nothing to do; just forward. + return true; + } + + const vector<int16>& block_sizes = function_->parameter_block_sizes(); + CHECK(!block_sizes.empty()); + + // Create local space for a copy of the parameters which will get mutated. + int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0); + vector<double> parameters_copy(parameters_size); + vector<double*> parameters_references_copy(block_sizes.size()); + parameters_references_copy[0] = ¶meters_copy[0]; + for (int block = 1; block < block_sizes.size(); ++block) { + parameters_references_copy[block] = parameters_references_copy[block - 1] + + block_sizes[block - 1]; + } + + // Copy the parameters into the local temp space. + for (int block = 0; block < block_sizes.size(); ++block) { + memcpy(parameters_references_copy[block], + parameters[block], + block_sizes[block] * sizeof(*parameters[block])); + } + + for (int block = 0; block < block_sizes.size(); ++block) { + if (!jacobians[block]) { + // No jacobian requested for this parameter / residual pair. + continue; + } + if (!EvaluateJacobianForParameterBlock(function_, + block_sizes[block], + block, + method_, + relative_step_size_, + residuals, + ¶meters_references_copy[0], + jacobians)) { + return false; + } + } + return true; + } + + private: + const CostFunction* function_; + RuntimeNumericDiffMethod method_; + double relative_step_size_; +}; + +} // namespace + +CostFunction* CreateRuntimeNumericDiffCostFunction( + const CostFunction* cost_function, + RuntimeNumericDiffMethod method, + double relative_step_size) { + return new RuntimeNumericDiffCostFunction(cost_function, + method, + relative_step_size); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.h b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.h new file mode 100644 index 00000000000..01b57f92ef3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.h @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Create CostFunctions as needed by the least squares framework with jacobians +// computed via numeric differentiation. +// +// To get a numerically differentiated cost function, define a subclass of +// CostFunction such that the Evaluate() function ignores the jacobian +// parameter. The numeric differentiation wrapper will fill in the jacobian +// parameter if nececssary by repeatedly calling the Evaluate() function with +// small changes to the appropriate parameters, and computing the slope. This +// implementation is not templated (hence the "Runtime" prefix), which is a bit +// slower than but is more convenient than the templated version in +// numeric_diff_cost_function.h +// +// The numerically differentiated version of a cost function for a cost function +// can be constructed as follows: +// +// CostFunction* cost_function = +// CreateRuntimeNumericDiffCostFunction(new MyCostFunction(...), +// CENTRAL, +// TAKE_OWNERSHIP); +// +// The central difference method is considerably more accurate; consider using +// to start and only after that works, trying forward difference. +// +// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives. + +#ifndef CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_ +#define CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_ + +#include "ceres/cost_function.h" + +namespace ceres { +namespace internal { + +enum RuntimeNumericDiffMethod { + CENTRAL, + FORWARD, +}; + +// Create a cost function that evaluates the derivative with finite differences. +// The base cost_function's implementation of Evaluate() only needs to fill in +// the "residuals" argument and not the "jacobians". Any data written to the +// jacobians by the base cost_function is overwritten. +// +// Forward difference or central difference is selected with CENTRAL or FORWARD. +// The relative eps, which determines the step size for forward and central +// differencing, is set with relative eps. Caller owns the resulting cost +// function, and the resulting cost function does not own the base cost +// function. +CostFunction *CreateRuntimeNumericDiffCostFunction( + const CostFunction *cost_function, + RuntimeNumericDiffMethod method, + double relative_eps); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc new file mode 100644 index 00000000000..2bc8cdd6bec --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc @@ -0,0 +1,285 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <algorithm> +#include <ctime> +#include <set> +#include <vector> +#include "Eigen/Dense" +#include "ceres/block_random_access_dense_matrix.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_random_access_sparse_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/detect_structure.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_complement_solver.h" +#include "ceres/suitesparse.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +LinearSolver::Summary SchurComplementSolver::SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + const time_t start_time = time(NULL); + if (!options_.constant_sparsity || (eliminator_.get() == NULL)) { + InitStorage(A->block_structure()); + DetectStructure(*A->block_structure(), + options_.num_eliminate_blocks, + &options_.row_block_size, + &options_.e_block_size, + &options_.f_block_size); + eliminator_.reset(CHECK_NOTNULL(SchurEliminatorBase::Create(options_))); + eliminator_->Init(options_.num_eliminate_blocks, A->block_structure()); + }; + const time_t init_time = time(NULL); + fill(x, x + A->num_cols(), 0.0); + + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = FAILURE; + eliminator_->Eliminate(A, b, per_solve_options.D, lhs_.get(), rhs_.get()); + const time_t eliminate_time = time(NULL); + + double* reduced_solution = x + A->num_cols() - lhs_->num_cols(); + const bool status = SolveReducedLinearSystem(reduced_solution); + const time_t solve_time = time(NULL); + + if (!status) { + return summary; + } + + eliminator_->BackSubstitute(A, b, per_solve_options.D, reduced_solution, x); + const time_t backsubstitute_time = time(NULL); + summary.termination_type = TOLERANCE; + + VLOG(2) << "time (sec) total: " << backsubstitute_time - start_time + << " init: " << init_time - start_time + << " eliminate: " << eliminate_time - init_time + << " solve: " << solve_time - eliminate_time + << " backsubstitute: " << backsubstitute_time - solve_time; + return summary; +} + +// Initialize a BlockRandomAccessDenseMatrix to store the Schur +// complement. +void DenseSchurComplementSolver::InitStorage( + const CompressedRowBlockStructure* bs) { + const int num_eliminate_blocks = options().num_eliminate_blocks; + const int num_col_blocks = bs->cols.size(); + + vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0); + for (int i = num_eliminate_blocks, j = 0; + i < num_col_blocks; + ++i, ++j) { + blocks[j] = bs->cols[i].size; + } + + set_lhs(new BlockRandomAccessDenseMatrix(blocks)); + set_rhs(new double[lhs()->num_rows()]); +} + +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessDenseMatrix. The linear system is solved using +// Eigen's Cholesky factorization. +bool DenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { + const BlockRandomAccessDenseMatrix* m = + down_cast<const BlockRandomAccessDenseMatrix*>(lhs()); + const int num_rows = m->num_rows(); + + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + return true; + } + + // TODO(sameeragarwal): Add proper error handling; this completely ignores + // the quality of the solution to the solve. + VectorRef(solution, num_rows) = + ConstMatrixRef(m->values(), num_rows, num_rows) + .selfadjointView<Eigen::Upper>() + .ldlt() + .solve(ConstVectorRef(rhs(), num_rows)); + + return true; +} + +#ifndef CERES_NO_SUITESPARSE +SparseSchurComplementSolver::SparseSchurComplementSolver( + const LinearSolver::Options& options) + : SchurComplementSolver(options), + symbolic_factor_(NULL) { +} + +SparseSchurComplementSolver::~SparseSchurComplementSolver() { + if (symbolic_factor_ != NULL) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } +} + +// Determine the non-zero blocks in the Schur Complement matrix, and +// initialize a BlockRandomAccessSparseMatrix object. +void SparseSchurComplementSolver::InitStorage( + const CompressedRowBlockStructure* bs) { + const int num_eliminate_blocks = options().num_eliminate_blocks; + const int num_col_blocks = bs->cols.size(); + const int num_row_blocks = bs->rows.size(); + + vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0); + for (int i = num_eliminate_blocks; i < num_col_blocks; ++i) { + blocks[i - num_eliminate_blocks] = bs->cols[i].size; + } + + set<pair<int, int> > block_pairs; + for (int i = 0; i < blocks.size(); ++i) { + block_pairs.insert(make_pair(i, i)); + } + + int r = 0; + while (r < num_row_blocks) { + int e_block_id = bs->rows[r].cells.front().block_id; + if (e_block_id >= num_eliminate_blocks) { + break; + } + vector<int> f_blocks; + + // Add to the chunk until the first block in the row is + // different than the one in the first row for the chunk. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs->rows[r]; + if (row.cells.front().block_id != e_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first + // block since it is the one to be eliminated. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + f_blocks.push_back(cell.block_id - num_eliminate_blocks); + } + } + + sort(f_blocks.begin(), f_blocks.end()); + f_blocks.erase(unique(f_blocks.begin(), f_blocks.end()), f_blocks.end()); + for (int i = 0; i < f_blocks.size(); ++i) { + for (int j = i + 1; j < f_blocks.size(); ++j) { + block_pairs.insert(make_pair(f_blocks[i], f_blocks[j])); + } + } + } + + // Remaing rows do not contribute to the chunks and directly go + // into the schur complement via an outer product. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs->rows[r]; + CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); + for (int i = 0; i < row.cells.size(); ++i) { + int r_block1_id = row.cells[i].block_id - num_eliminate_blocks; + for (int j = 0; j < row.cells.size(); ++j) { + int r_block2_id = row.cells[j].block_id - num_eliminate_blocks; + if (r_block1_id <= r_block2_id) { + block_pairs.insert(make_pair(r_block1_id, r_block2_id)); + } + } + } + } + + set_lhs(new BlockRandomAccessSparseMatrix(blocks, block_pairs)); + set_rhs(new double[lhs()->num_rows()]); +} + +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessSparseMatrix. The linear system is solved using +// CHOLMOD's sparse cholesky factorization routines. +bool SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { + // Extract the TripletSparseMatrix that is used for actually storing S. + TripletSparseMatrix* tsm = + const_cast<TripletSparseMatrix*>( + down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); + + const int num_rows = tsm->num_rows(); + + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + return true; + } + + cholmod_sparse* cholmod_lhs = ss_.CreateSparseMatrix(tsm); + // The matrix is symmetric, and the upper triangular part of the + // matrix contains the values. + cholmod_lhs->stype = 1; + + cholmod_dense* cholmod_rhs = + ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows); + + // Symbolic factorization is computed if we don't already have one handy. + if (symbolic_factor_ == NULL) { + symbolic_factor_ = ss_.AnalyzeCholesky(cholmod_lhs); + } + + cholmod_dense* cholmod_solution = + ss_.SolveCholesky(cholmod_lhs, symbolic_factor_, cholmod_rhs); + + ss_.Free(cholmod_lhs); + cholmod_lhs = NULL; + ss_.Free(cholmod_rhs); + cholmod_rhs = NULL; + + // If sparsity is not constant across calls, then reset the symbolic + // factorization. + if (!options().constant_sparsity) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } + + if (cholmod_solution == NULL) { + LOG(ERROR) << "CHOLMOD solve failed."; + return false; + } + + VectorRef(solution, num_rows) + = VectorRef(static_cast<double*>(cholmod_solution->x), num_rows); + ss_.Free(cholmod_solution); + return true; +} +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h new file mode 100644 index 00000000000..039bc09e3ce --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h @@ -0,0 +1,182 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ +#define CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ + +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/suitesparse.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrixBase; + +// Base class for Schur complement based linear least squares +// solvers. It assumes that the input linear system Ax = b can be +// partitioned into +// +// E y + F z = b +// +// Where x = [y;z] is a partition of the variables. The paritioning +// of the variables is such that, E'E is a block diagonal +// matrix. Further, the rows of A are ordered so that for every +// variable block in y, all the rows containing that variable block +// occur as a vertically contiguous block. i.e the matrix A looks like +// +// E F +// A = [ y1 0 0 0 | z1 0 0 0 z5] +// [ y1 0 0 0 | z1 z2 0 0 0] +// [ 0 y2 0 0 | 0 0 z3 0 0] +// [ 0 0 y3 0 | z1 z2 z3 z4 z5] +// [ 0 0 y3 0 | z1 0 0 0 z5] +// [ 0 0 0 y4 | 0 0 0 0 z5] +// [ 0 0 0 y4 | 0 z2 0 0 0] +// [ 0 0 0 y4 | 0 0 0 0 0] +// [ 0 0 0 0 | z1 0 0 0 0] +// [ 0 0 0 0 | 0 0 z3 z4 z5] +// +// This structure should be reflected in the corresponding +// CompressedRowBlockStructure object associated with A. The linear +// system Ax = b should either be well posed or the array D below +// should be non-null and the diagonal matrix corresponding to it +// should be non-singular. +// +// SchurComplementSolver has two sub-classes. +// +// DenseSchurComplementSolver: For problems where the Schur complement +// matrix is small and dense, or if CHOLMOD/SuiteSparse is not +// installed. For structure from motion problems, this is solver can +// be used for problems with upto a few hundred cameras. +// +// SparseSchurComplementSolver: For problems where the Schur +// complement matrix is large and sparse. It requires that +// CHOLMOD/SuiteSparse be installed, as it uses CHOLMOD to find a +// sparse Cholesky factorization of the Schur complement. This solver +// can be used for solving structure from motion problems with tens of +// thousands of cameras, though depending on the exact sparsity +// structure, it maybe better to use an iterative solver. +// +// The two solvers can be instantiated by calling +// LinearSolver::CreateLinearSolver with LinearSolver::Options::type +// set to DENSE_SCHUR and SPARSE_SCHUR +// respectively. LinearSolver::Options::num_eliminate_blocks should be +// at least 1. +class SchurComplementSolver : public BlockSparseMatrixBaseSolver { + public: + explicit SchurComplementSolver(const LinearSolver::Options& options) + : options_(options) { + CHECK_GT(options.num_eliminate_blocks, 0); + } + + // LinearSolver methods + virtual ~SchurComplementSolver() {} + virtual LinearSolver::Summary SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + protected: + const LinearSolver::Options& options() const { return options_; } + + const BlockRandomAccessMatrix* lhs() const { return lhs_.get(); } + void set_lhs(BlockRandomAccessMatrix* lhs) { lhs_.reset(lhs); } + const double* rhs() const { return rhs_.get(); } + void set_rhs(double* rhs) { rhs_.reset(rhs); } + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs) = 0; + virtual bool SolveReducedLinearSystem(double* solution) = 0; + + LinearSolver::Options options_; + + scoped_ptr<SchurEliminatorBase> eliminator_; + scoped_ptr<BlockRandomAccessMatrix> lhs_; + scoped_array<double> rhs_; + + DISALLOW_COPY_AND_ASSIGN(SchurComplementSolver); +}; + +// Dense Cholesky factorization based solver. +class DenseSchurComplementSolver : public SchurComplementSolver { + public: + explicit DenseSchurComplementSolver(const LinearSolver::Options& options) + : SchurComplementSolver(options) {} + virtual ~DenseSchurComplementSolver() {} + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs); + virtual bool SolveReducedLinearSystem(double* solution); + + DISALLOW_COPY_AND_ASSIGN(DenseSchurComplementSolver); +}; + +#ifndef CERES_NO_SUITESPARSE +// Sparse Cholesky factorization based solver. +class SparseSchurComplementSolver : public SchurComplementSolver { + public: + explicit SparseSchurComplementSolver(const LinearSolver::Options& options); + virtual ~SparseSchurComplementSolver(); + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs); + virtual bool SolveReducedLinearSystem(double* solution); + + + SuiteSparse ss_; + // Symbolic factorization of the reduced linear system. Precomputed + // once and reused if constant_sparsity_ is true. + cholmod_factor* symbolic_factor_; + DISALLOW_COPY_AND_ASSIGN(SparseSchurComplementSolver); +}; +#else // CERES_NO_SUITESPARSE +class SparseSchurComplementSolver : public SchurComplementSolver { + public: + explicit SparseSchurComplementSolver(const LinearSolver::Options& options) + : SchurComplementSolver(options) { + LOG(FATAL) << "SPARSE_SCHUR is not available. Please " + "build Ceres with SuiteSparse."; + } + + virtual ~SparseSchurComplementSolver() {} +}; +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.cc new file mode 100644 index 00000000000..44f5be3b4e9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.cc @@ -0,0 +1,141 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_template_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +SchurEliminatorBase* +SchurEliminatorBase::Create(const LinearSolver::Options& options) { +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 2)) { + return new SchurEliminator<2, 2, 2>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 2, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 2, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<2, 2, Dynamic>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 3, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 3, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 9)) { + return new SchurEliminator<2, 3, 9>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<2, 3, Dynamic>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 4, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 4, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<2, 4, Dynamic>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 2)) { + return new SchurEliminator<4, 4, 2>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new SchurEliminator<4, 4, 3>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new SchurEliminator<4, 4, 4>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<4, 4, Dynamic>(options); + } + if ((options.row_block_size == Dynamic) && + (options.e_block_size == Dynamic) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<Dynamic, Dynamic, Dynamic>(options); + } + +#endif + VLOG(1) << "Template specializations not found for <" + << options.row_block_size << "," + << options.e_block_size << "," + << options.f_block_size << ">"; + return new SchurEliminator<Dynamic, Dynamic, Dynamic>(options); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h new file mode 100644 index 00000000000..c24fe435f54 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h @@ -0,0 +1,339 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_SCHUR_ELIMINATOR_H_ +#define CERES_INTERNAL_SCHUR_ELIMINATOR_H_ + +#include <map> +#include <vector> +#include "ceres/mutex.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/linear_solver.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// Classes implementing the SchurEliminatorBase interface implement +// variable elimination for linear least squares problems. Assuming +// that the input linear system Ax = b can be partitioned into +// +// E y + F z = b +// +// Where x = [y;z] is a partition of the variables. The paritioning +// of the variables is such that, E'E is a block diagonal matrix. Or +// in other words, the parameter blocks in E form an independent set +// of the of the graph implied by the block matrix A'A. Then, this +// class provides the functionality to compute the Schur complement +// system +// +// S z = r +// +// where +// +// S = F'F - F'E (E'E)^{-1} E'F and r = F'b - F'E(E'E)^(-1) E'b +// +// This is the Eliminate operation, i.e., construct the linear system +// obtained by eliminating the variables in E. +// +// The eliminator also provides the reverse functionality, i.e. given +// values for z it can back substitute for the values of y, by solving the +// linear system +// +// Ey = b - F z +// +// which is done by observing that +// +// y = (E'E)^(-1) [E'b - E'F z] +// +// The eliminator has a number of requirements. +// +// The rows of A are ordered so that for every variable block in y, +// all the rows containing that variable block occur as a vertically +// contiguous block. i.e the matrix A looks like +// +// E F chunk +// A = [ y1 0 0 0 | z1 0 0 0 z5] 1 +// [ y1 0 0 0 | z1 z2 0 0 0] 1 +// [ 0 y2 0 0 | 0 0 z3 0 0] 2 +// [ 0 0 y3 0 | z1 z2 z3 z4 z5] 3 +// [ 0 0 y3 0 | z1 0 0 0 z5] 3 +// [ 0 0 0 y4 | 0 0 0 0 z5] 4 +// [ 0 0 0 y4 | 0 z2 0 0 0] 4 +// [ 0 0 0 y4 | 0 0 0 0 0] 4 +// [ 0 0 0 0 | z1 0 0 0 0] non chunk blocks +// [ 0 0 0 0 | 0 0 z3 z4 z5] non chunk blocks +// +// This structure should be reflected in the corresponding +// CompressedRowBlockStructure object associated with A. The linear +// system Ax = b should either be well posed or the array D below +// should be non-null and the diagonal matrix corresponding to it +// should be non-singular. For simplicity of exposition only the case +// with a null D is described. +// +// The usual way to do the elimination is as follows. Starting with +// +// E y + F z = b +// +// we can form the normal equations, +// +// E'E y + E'F z = E'b +// F'E y + F'F z = F'b +// +// multiplying both sides of the first equation by (E'E)^(-1) and then +// by F'E we get +// +// F'E y + F'E (E'E)^(-1) E'F z = F'E (E'E)^(-1) E'b +// F'E y + F'F z = F'b +// +// now subtracting the two equations we get +// +// [FF' - F'E (E'E)^(-1) E'F] z = F'b - F'E(E'E)^(-1) E'b +// +// Instead of forming the normal equations and operating on them as +// general sparse matrices, the algorithm here deals with one +// parameter block in y at a time. The rows corresponding to a single +// parameter block yi are known as a chunk, and the algorithm operates +// on one chunk at a time. The mathematics remains the same since the +// reduced linear system can be shown to be the sum of the reduced +// linear systems for each chunk. This can be seen by observing two +// things. +// +// 1. E'E is a block diagonal matrix. +// +// 2. When E'F is computed, only the terms within a single chunk +// interact, i.e for y1 column blocks when transposed and multiplied +// with F, the only non-zero contribution comes from the blocks in +// chunk1. +// +// Thus, the reduced linear system +// +// FF' - F'E (E'E)^(-1) E'F +// +// can be re-written as +// +// sum_k F_k F_k' - F_k'E_k (E_k'E_k)^(-1) E_k' F_k +// +// Where the sum is over chunks and E_k'E_k is dense matrix of size y1 +// x y1. +// +// Advanced usage. Uptil now it has been assumed that the user would +// be interested in all of the Schur Complement S. However, it is also +// possible to use this eliminator to obtain an arbitrary submatrix of +// the full Schur complement. When the eliminator is generating the +// blocks of S, it asks the RandomAccessBlockMatrix instance passed to +// it if it has storage for that block. If it does, the eliminator +// computes/updates it, if not it is skipped. This is useful when one +// is interested in constructing a preconditioner based on the Schur +// Complement, e.g., computing the block diagonal of S so that it can +// be used as a preconditioner for an Iterative Substructuring based +// solver [See Agarwal et al, Bundle Adjustment in the Large, ECCV +// 2008 for an example of such use]. +// +// Example usage: Please see schur_complement_solver.cc +class SchurEliminatorBase { + public: + virtual ~SchurEliminatorBase() {} + + // Initialize the eliminator. It is the user's responsibilty to call + // this function before calling Eliminate or BackSubstitute. It is + // also the caller's responsibilty to ensure that the + // CompressedRowBlockStructure object passed to this method is the + // same one (or is equivalent to) the one associated with the + // BlockSparseMatrixBase objects below. + virtual void Init(int num_eliminate_blocks, + const CompressedRowBlockStructure* bs) = 0; + + // Compute the Schur complement system from the augmented linear + // least squares problem [A;D] x = [b;0]. The left hand side and the + // right hand side of the reduced linear system are returned in lhs + // and rhs respectively. + // + // It is the caller's responsibility to construct and initialize + // lhs. Depending upon the structure of the lhs object passed here, + // the full or a submatrix of the Schur complement will be computed. + // + // Since the Schur complement is a symmetric matrix, only the upper + // triangular part of the Schur complement is computed. + virtual void Eliminate(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs) = 0; + + // Given values for the variables z in the F block of A, solve for + // the optimal values of the variables y corresponding to the E + // block in A. + virtual void BackSubstitute(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + const double* z, + double* y) = 0; + // Factory + static SchurEliminatorBase* Create(const LinearSolver::Options& options); +}; + +// Templated implementation of the SchurEliminatorBase interface. The +// templating is on the sizes of the row, e and f blocks sizes in the +// input matrix. In many problems, the sizes of one or more of these +// blocks are constant, in that case, its worth passing these +// parameters as template arguments so that they are visible to the +// compiler and can be used for compile time optimization of the low +// level linear algebra routines. +// +// This implementation is mulithreaded using OpenMP. The level of +// parallelism is controlled by LinearSolver::Options::num_threads. +template <int kRowBlockSize = Dynamic, + int kEBlockSize = Dynamic, + int kFBlockSize = Dynamic > +class SchurEliminator : public SchurEliminatorBase { + public: + explicit SchurEliminator(const LinearSolver::Options& options) + : num_threads_(options.num_threads) { + } + + // SchurEliminatorBase Interface + virtual ~SchurEliminator(); + virtual void Init(int num_eliminate_blocks, + const CompressedRowBlockStructure* bs); + virtual void Eliminate(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs); + virtual void BackSubstitute(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + const double* z, + double* y); + + private: + // Chunk objects store combinatorial information needed to + // efficiently eliminate a whole chunk out of the least squares + // problem. Consider the first chunk in the example matrix above. + // + // [ y1 0 0 0 | z1 0 0 0 z5] + // [ y1 0 0 0 | z1 z2 0 0 0] + // + // One of the intermediate quantities that needs to be calculated is + // for each row the product of the y block transposed with the + // non-zero z block, and the sum of these blocks across rows. A + // temporary array "buffer_" is used for computing and storing them + // and the buffer_layout maps the indices of the z-blocks to + // position in the buffer_ array. The size of the chunk is the + // number of row blocks/residual blocks for the particular y block + // being considered. + // + // For the example chunk shown above, + // + // size = 2 + // + // The entries of buffer_layout will be filled in the following order. + // + // buffer_layout[z1] = 0 + // buffer_layout[z5] = y1 * z1 + // buffer_layout[z2] = y1 * z1 + y1 * z5 + typedef map<int, int> BufferLayoutType; + struct Chunk { + Chunk() : size(0) {} + int size; + int start; + BufferLayoutType buffer_layout; + }; + + void ChunkDiagonalBlockAndGradient( + const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* eet, + typename EigenTypes<kEBlockSize>::Vector* g, + double* buffer, + BlockRandomAccessMatrix* lhs); + + void UpdateRhs(const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + const Vector& inverse_ete_g, + double* rhs); + + void ChunkOuterProduct(const CompressedRowBlockStructure* bs, + const Matrix& inverse_eet, + const double* buffer, + const BufferLayoutType& buffer_layout, + BlockRandomAccessMatrix* lhs); + void EBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs); + + + void NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + BlockRandomAccessMatrix* lhs, + double* rhs); + + void NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs); + + int num_eliminate_blocks_; + + // Block layout of the columns of the reduced linear system. Since + // the f blocks can be of varying size, this vector stores the + // position of each f block in the row/col of the reduced linear + // system. Thus lhs_row_layout_[i] is the row/col position of the + // i^th f block. + vector<int> lhs_row_layout_; + + // Combinatorial structure of the chunks in A. For more information + // see the documentation of the Chunk object above. + vector<Chunk> chunks_; + + // Buffer to store the products of the y and z blocks generated + // during the elimination phase. + scoped_array<double> buffer_; + int buffer_size_; + int num_threads_; + int uneliminated_row_begins_; + + // Locks for the blocks in the right hand side of the reduced linear + // system. + vector<Mutex*> rhs_locks_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_ELIMINATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h new file mode 100644 index 00000000000..a388d005424 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h @@ -0,0 +1,702 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// TODO(sameeragarwal): row_block_counter can perhaps be replaced by +// Chunk::start ? + +#ifndef CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ +#define CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +// Eigen has an internal threshold switching between different matrix +// multiplication algorithms. In particular for matrices larger than +// EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD it uses a cache friendly +// matrix matrix product algorithm that has a higher setup cost. For +// matrix sizes close to this threshold, especially when the matrices +// are thin and long, the default choice may not be optimal. This is +// the case for us, as the default choice causes a 30% performance +// regression when we moved from Eigen2 to Eigen3. +#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10 + +#include <algorithm> +#include <map> +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/map_util.h" +#include "ceres/schur_eliminator.h" +#include "ceres/stl_util.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::~SchurEliminator() { + STLDeleteElements(&rhs_locks_); +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) { + CHECK_GT(num_eliminate_blocks, 0) + << "SchurComplementSolver cannot be initialized with " + << "num_eliminate_blocks = 0."; + + num_eliminate_blocks_ = num_eliminate_blocks; + + const int num_col_blocks = bs->cols.size(); + const int num_row_blocks = bs->rows.size(); + + buffer_size_ = 1; + chunks_.clear(); + lhs_row_layout_.clear(); + + int lhs_num_rows = 0; + // Add a map object for each block in the reduced linear system + // and build the row/column block structure of the reduced linear + // system. + lhs_row_layout_.resize(num_col_blocks - num_eliminate_blocks_); + for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { + lhs_row_layout_[i - num_eliminate_blocks_] = lhs_num_rows; + lhs_num_rows += bs->cols[i].size; + } + + int r = 0; + // Iterate over the row blocks of A, and detect the chunks. The + // matrix should already have been ordered so that all rows + // containing the same y block are vertically contiguous. Along + // the way also compute the amount of space each chunk will need + // to perform the elimination. + while (r < num_row_blocks) { + const int chunk_block_id = bs->rows[r].cells.front().block_id; + if (chunk_block_id >= num_eliminate_blocks_) { + break; + } + + chunks_.push_back(Chunk()); + Chunk& chunk = chunks_.back(); + chunk.size = 0; + chunk.start = r; + int buffer_size = 0; + const int e_block_size = bs->cols[chunk_block_id].size; + + // Add to the chunk until the first block in the row is + // different than the one in the first row for the chunk. + while (r + chunk.size < num_row_blocks) { + const CompressedRow& row = bs->rows[r + chunk.size]; + if (row.cells.front().block_id != chunk_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first + // block since it is the one to be eliminated. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + if (InsertIfNotPresent( + &(chunk.buffer_layout), cell.block_id, buffer_size)) { + buffer_size += e_block_size * bs->cols[cell.block_id].size; + } + } + + buffer_size_ = max(buffer_size, buffer_size_); + ++chunk.size; + } + + CHECK_GT(chunk.size, 0); + r += chunk.size; + } + const Chunk& chunk = chunks_.back(); + + uneliminated_row_begins_ = chunk.start + chunk.size; + if (num_threads_ > 1) { + random_shuffle(chunks_.begin(), chunks_.end()); + } + + buffer_.reset(new double[buffer_size_ * num_threads_]); + + STLDeleteElements(&rhs_locks_); + rhs_locks_.resize(num_col_blocks - num_eliminate_blocks_); + for (int i = 0; i < num_col_blocks - num_eliminate_blocks_; ++i) { + rhs_locks_[i] = new Mutex; + } + + VLOG(1) << "Eliminator threads: " << num_threads_; +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +Eliminate(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs) { + if (lhs->num_rows() > 0) { + lhs->SetZero(); + VectorRef(rhs, lhs->num_rows()).setZero(); + } + + const CompressedRowBlockStructure* bs = A->block_structure(); + const int num_col_blocks = bs->cols.size(); + + // Add the diagonal to the schur complement. + if (D != NULL) { +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { + const int block_id = i - num_eliminate_blocks_; + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block_id, block_id, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + const int block_size = bs->cols[i].size; + typename EigenTypes<kFBlockSize>::ConstVectorRef + diag(D + bs->cols[i].position, block_size); + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size, block_size).diagonal() + += diag.array().square().matrix(); + } + } + } + + // Eliminate y blocks one chunk at a time. For each chunk,x3 + // compute the entries of the normal equations and the gradient + // vector block corresponding to the y block and then apply + // Gaussian elimination to them. The matrix ete stores the normal + // matrix corresponding to the block being eliminated and array + // buffer_ contains the non-zero blocks in the row corresponding + // to this y block in the normal equations. This computation is + // done in ChunkDiagonalBlockAndGradient. UpdateRhs then applies + // gaussian elimination to the rhs of the normal equations, + // updating the rhs of the reduced linear system by modifying rhs + // blocks for all the z blocks that share a row block/residual + // term with the y block. EliminateRowOuterProduct does the + // corresponding operation for the lhs of the reduced linear + // system. +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = 0; i < chunks_.size(); ++i) { +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + double* buffer = buffer_.get() + thread_id * buffer_size_; + const Chunk& chunk = chunks_[i]; + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + const int e_block_size = bs->cols[e_block_id].size; + + VectorRef(buffer, buffer_size_).setZero(); + + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix + ete(e_block_size, e_block_size); + + if (D != NULL) { + const typename EigenTypes<kEBlockSize>::ConstVectorRef + diag(D + bs->cols[e_block_id].position, e_block_size); + ete = diag.array().square().matrix().asDiagonal(); + } else { + ete.setZero(); + } + + typename EigenTypes<kEBlockSize>::Vector g(e_block_size); + g.setZero(); + + // We are going to be computing + // + // S += F'F - F'E(E'E)^{-1}E'F + // + // for each Chunk. The computation is broken down into a number of + // function calls as below. + + // Compute the outer product of the e_blocks with themselves (ete + // = E'E). Compute the product of the e_blocks with the + // corresonding f_blocks (buffer = E'F), the gradient of the terms + // in this chunk (g) and add the outer product of the f_blocks to + // Schur complement (S += F'F). + ChunkDiagonalBlockAndGradient( + chunk, A, b, chunk.start, &ete, &g, buffer, lhs); + + // Normally one wouldn't compute the inverse explicitly, but + // e_block_size will typically be a small number like 3, in + // which case its much faster to compute the inverse once and + // use it to multiply other matrices/vectors instead of doing a + // Solve call over and over again. + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix inverse_ete = + ete + .template selfadjointView<Eigen::Upper>() + .ldlt() + .solve(Matrix::Identity(e_block_size, e_block_size)); + + // For the current chunk compute and update the rhs of the reduced + // linear system. + // + // rhs = F'b - F'E(E'E)^(-1) E'b + UpdateRhs(chunk, A, b, chunk.start, inverse_ete * g, rhs); + + // S -= F'E(E'E)^{-1}E'F + ChunkOuterProduct(bs, inverse_ete, buffer, chunk.buffer_layout, lhs); + } + + // For rows with no e_blocks, the schur complement update reduces to + // S += F'F. + NoEBlockRowsUpdate(A, b, uneliminated_row_begins_, lhs, rhs); +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +BackSubstitute(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + const double* z, + double* y) { + const CompressedRowBlockStructure* bs = A->block_structure(); +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = 0; i < chunks_.size(); ++i) { + const Chunk& chunk = chunks_[i]; + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + const int e_block_size = bs->cols[e_block_id].size; + + typename EigenTypes<kEBlockSize>::VectorRef y_block( + y + bs->cols[e_block_id].position, e_block_size); + + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix + ete(e_block_size, e_block_size); + if (D != NULL) { + const typename EigenTypes<kEBlockSize>::ConstVectorRef + diag(D + bs->cols[e_block_id].position, e_block_size); + ete = diag.array().square().matrix().asDiagonal(); + } else { + ete.setZero(); + } + + for (int j = 0; j < chunk.size; ++j) { + const CompressedRow& row = bs->rows[chunk.start + j]; + const double* row_values = A->RowBlockValues(chunk.start + j); + const Cell& e_cell = row.cells.front(); + DCHECK_EQ(e_block_id, e_cell.block_id); + const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef + e_block(row_values + e_cell.position, + row.block.size, + e_block_size); + + typename EigenTypes<kRowBlockSize>::Vector + sj = + typename EigenTypes<kRowBlockSize>::ConstVectorRef + (b + bs->rows[chunk.start + j].block.position, + row.block.size); + + for (int c = 1; c < row.cells.size(); ++c) { + const int f_block_id = row.cells[c].block_id; + const int f_block_size = bs->cols[f_block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + f_block(row_values + row.cells[c].position, + row.block.size, f_block_size); + const int r_block = f_block_id - num_eliminate_blocks_; + + sj -= f_block * + typename EigenTypes<kFBlockSize>::ConstVectorRef + (z + lhs_row_layout_[r_block], f_block_size); + } + + y_block += e_block.transpose() * sj; + ete.template selfadjointView<Eigen::Upper>() + .rankUpdate(e_block.transpose(), 1.0); + } + + y_block = + ete + .template selfadjointView<Eigen::Upper>() + .ldlt() + .solve(y_block); + } +} + +// Update the rhs of the reduced linear system. Compute +// +// F'b - F'E(E'E)^(-1) E'b +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +UpdateRhs(const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + const Vector& inverse_ete_g, + double* rhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const int e_block_size = inverse_ete_g.rows(); + int b_pos = bs->rows[row_block_counter].block.position; + for (int j = 0; j < chunk.size; ++j) { + const CompressedRow& row = bs->rows[row_block_counter + j]; + const double *row_values = A->RowBlockValues(row_block_counter + j); + const Cell& e_cell = row.cells.front(); + + const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef + e_block(row_values + e_cell.position, + row.block.size, + e_block_size); + + const typename EigenTypes<kRowBlockSize>::Vector + sj = + typename EigenTypes<kRowBlockSize>::ConstVectorRef + (b + b_pos, row.block.size) - e_block * (inverse_ete_g); + + for (int c = 1; c < row.cells.size(); ++c) { + const int block_id = row.cells[c].block_id; + const int block_size = bs->cols[block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + b(row_values + row.cells[c].position, + row.block.size, block_size); + + const int block = block_id - num_eliminate_blocks_; + MutexLock l(rhs_locks_[block]); + typename EigenTypes<kFBlockSize>::VectorRef + (rhs + lhs_row_layout_[block], block_size).noalias() + += b.transpose() * sj; + } + b_pos += row.block.size; + } +} + +// Given a Chunk - set of rows with the same e_block, e.g. in the +// following Chunk with two rows. +// +// E F +// [ y11 0 0 0 | z11 0 0 0 z51] +// [ y12 0 0 0 | z12 z22 0 0 0] +// +// this function computes twp matrices. The diagonal block matrix +// +// ete = y11 * y11' + y12 * y12' +// +// and the off diagonal blocks in the Guass Newton Hessian. +// +// buffer = [y11'(z11 + z12), y12' * z22, y11' * z51] +// +// which are zero compressed versions of the block sparse matrices E'E +// and E'F. +// +// and the gradient of the e_block, E'b. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +ChunkDiagonalBlockAndGradient( + const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* ete, + typename EigenTypes<kEBlockSize>::Vector* g, + double* buffer, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + + int b_pos = bs->rows[row_block_counter].block.position; + const int e_block_size = ete->rows(); + + // Iterate over the rows in this chunk, for each row, compute the + // contribution of its F blocks to the Schur complement, the + // contribution of its E block to the matrix EE' (ete), and the + // corresponding block in the gradient vector. + for (int j = 0; j < chunk.size; ++j) { + const CompressedRow& row = bs->rows[row_block_counter + j]; + const double *row_values = A->RowBlockValues(row_block_counter + j); + + if (row.cells.size() > 1) { + EBlockRowOuterProduct(A, row_block_counter + j, lhs); + } + + // Extract the e_block, ETE += E_i' E_i + const Cell& e_cell = row.cells.front(); + const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef + e_block(row_values + e_cell.position, + row.block.size, + e_block_size); + + ete->template selfadjointView<Eigen::Upper>() + .rankUpdate(e_block.transpose(), 1.0); + + // g += E_i' b_i + g->noalias() += e_block.transpose() * + typename EigenTypes<kRowBlockSize>::ConstVectorRef + (b + b_pos, row.block.size); + + // buffer = E'F. This computation is done by iterating over the + // f_blocks for each row in the chunk. + for (int c = 1; c < row.cells.size(); ++c) { + const int f_block_id = row.cells[c].block_id; + const int f_block_size = bs->cols[f_block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + f_block(row_values + row.cells[c].position, + row.block.size, f_block_size); + + double* buffer_ptr = + buffer + FindOrDie(chunk.buffer_layout, f_block_id); + + typename EigenTypes<kEBlockSize, kFBlockSize>::MatrixRef + (buffer_ptr, e_block_size, f_block_size).noalias() + += e_block.transpose() * f_block; + } + b_pos += row.block.size; + } +} + +// Compute the outer product F'E(E'E)^{-1}E'F and subtract it from the +// Schur complement matrix, i.e +// +// S -= F'E(E'E)^{-1}E'F. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +ChunkOuterProduct(const CompressedRowBlockStructure* bs, + const Matrix& inverse_ete, + const double* buffer, + const BufferLayoutType& buffer_layout, + BlockRandomAccessMatrix* lhs) { + // This is the most computationally expensive part of this + // code. Profiling experiments reveal that the bottleneck is not the + // computation of the right-hand matrix product, but memory + // references to the left hand side. + const int e_block_size = inverse_ete.rows(); + BufferLayoutType::const_iterator it1 = buffer_layout.begin(); + // S(i,j) -= bi' * ete^{-1} b_j + for (; it1 != buffer_layout.end(); ++it1) { + const int block1 = it1->first - num_eliminate_blocks_; + const int block1_size = bs->cols[it1->first].size; + + const typename EigenTypes<kEBlockSize, kFBlockSize>::ConstMatrixRef + b1(buffer + it1->second, e_block_size, block1_size); + const typename EigenTypes<kFBlockSize, kEBlockSize>::Matrix + b1_transpose_inverse_ete = b1.transpose() * inverse_ete; + + BufferLayoutType::const_iterator it2 = it1; + for (; it2 != buffer_layout.end(); ++it2) { + const int block2 = it2->first - num_eliminate_blocks_; + + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + const int block2_size = bs->cols[it2->first].size; + const typename EigenTypes<kEBlockSize, kFBlockSize>::ConstMatrixRef + b2(buffer + it2->second, e_block_size, block2_size); + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + + // We explicitly construct a block object here instead of using + // m.block(), as m.block() variant of the constructor does not + // allow mixing of template sizing and runtime sizing parameters + // like the Matrix class does. + Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize> + block(m, r, c, block1_size, block2_size); + block.noalias() -= b1_transpose_inverse_ete * b2; + } + } +} + +// For rows with no e_blocks, the schur complement update reduces to S +// += F'F. This function iterates over the rows of A with no e_block, +// and calls NoEBlockRowOuterProduct on each row. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + BlockRandomAccessMatrix* lhs, + double* rhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + for (; row_block_counter < bs->rows.size(); ++row_block_counter) { + const CompressedRow& row = bs->rows[row_block_counter]; + const double *row_values = A->RowBlockValues(row_block_counter); + for (int c = 0; c < row.cells.size(); ++c) { + const int block_id = row.cells[c].block_id; + const int block_size = bs->cols[block_id].size; + const int block = block_id - num_eliminate_blocks_; + VectorRef(rhs + lhs_row_layout_[block], block_size).noalias() + += (ConstMatrixRef(row_values + row.cells[c].position, + row.block.size, block_size).transpose() * + ConstVectorRef(b + row.block.position, row.block.size)); + } + NoEBlockRowOuterProduct(A, row_block_counter, lhs); + } +} + + +// A row r of A, which has no e_blocks gets added to the Schur +// Complement as S += r r'. This function is responsible for computing +// the contribution of a single row r to the Schur complement. It is +// very similar in structure to EBlockRowOuterProduct except for +// one difference. It does not use any of the template +// parameters. This is because the algorithm used for detecting the +// static structure of the matrix A only pays attention to rows with +// e_blocks. This is becase rows without e_blocks are rare and +// typically arise from regularization terms in the original +// optimization problem, and have a very different structure than the +// rows with e_blocks. Including them in the static structure +// detection will lead to most template parameters being set to +// dynamic. Since the number of rows without e_blocks is small, the +// lack of templating is not an issue. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRow& row = bs->rows[row_block_index]; + const double *row_values = A->RowBlockValues(row_block_index); + for (int i = 0; i < row.cells.size(); ++i) { + const int block1 = row.cells[i].block_id - num_eliminate_blocks_; + DCHECK_GE(block1, 0); + + const int block1_size = bs->cols[row.cells[i].block_id].size; + const ConstMatrixRef b1(row_values + row.cells[i].position, + row.block.size, block1_size); + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block1, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block1_size, block1_size) + .selfadjointView<Eigen::Upper>() + .rankUpdate(b1.transpose(), 1.0); + } + + for (int j = i + 1; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks_; + DCHECK_GE(block2, 0); + DCHECK_LT(block1, block2); + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + const int block2_size = bs->cols[row.cells[j].block_id].size; + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block1_size, block2_size).noalias() += + b1.transpose() * ConstMatrixRef(row_values + row.cells[j].position, + row.block.size, + block2_size); + } + } +} + +// For a row with an e_block, compute the contribition S += F'F. This +// function has the same structure as NoEBlockRowOuterProduct, except +// that this function uses the template parameters. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +EBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRow& row = bs->rows[row_block_index]; + const double *row_values = A->RowBlockValues(row_block_index); + for (int i = 1; i < row.cells.size(); ++i) { + const int block1 = row.cells[i].block_id - num_eliminate_blocks_; + DCHECK_GE(block1, 0); + + const int block1_size = bs->cols[row.cells[i].block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + b1(row_values + row.cells[i].position, + row.block.size, block1_size); + { + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block1, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + + Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize> + block(m, r, c, block1_size, block1_size); + block.template selfadjointView<Eigen::Upper>() + .rankUpdate(b1.transpose(), 1.0); + } + + for (int j = i + 1; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks_; + DCHECK_GE(block2, 0); + DCHECK_LT(block1, block2); + const int block2_size = bs->cols[row.cells[j].block_id].size; + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + b2(row_values + row.cells[j].position, + row.block.size, + block2_size); + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize> + block(m, r, c, block1_size, block2_size); + block.noalias() += b1.transpose() * b2; + } + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc new file mode 100644 index 00000000000..c4fc1da3c2f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc @@ -0,0 +1,113 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/schur_ordering.h" + +#include <glog/logging.h> +#include "ceres/graph.h" +#include "ceres/graph_algorithms.h" +#include "ceres/map_util.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/internal/scoped_ptr.h" + +CERES_HASH_NAMESPACE_START + +// Allow us to hash pointers as if they were int's +template<> struct hash< ::ceres::internal::ParameterBlock*> { + size_t operator()(::ceres::internal::ParameterBlock* x) const { + return reinterpret_cast<size_t>(x); + } +}; + +CERES_HASH_NAMESPACE_END + +namespace ceres { +namespace internal { + +int ComputeSchurOrdering(const Program& program, + vector<ParameterBlock*>* ordering) { + CHECK_NOTNULL(ordering)->clear(); + + scoped_ptr<Graph< ParameterBlock*> > graph( + CHECK_NOTNULL(CreateHessianGraph(program))); + int independent_set_size = + IndependentSetOrdering<ParameterBlock*>(*graph, ordering); + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + + // Add the excluded blocks to back of the ordering vector. + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + if (parameter_block->IsConstant()) { + ordering->push_back(parameter_block); + } + } + + return independent_set_size; +} + +Graph<ParameterBlock*>* +CreateHessianGraph(const Program& program) { + Graph<ParameterBlock*>* graph = new Graph<ParameterBlock*>; + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + if (!parameter_block->IsConstant()) { + graph->AddVertex(parameter_block); + } + } + + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + ParameterBlock* const* parameter_blocks = + residual_block->parameter_blocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + if (parameter_blocks[j]->IsConstant()) { + continue; + } + + for (int k = j + 1; k < num_parameter_blocks; ++k) { + if (parameter_blocks[k]->IsConstant()) { + continue; + } + + graph->AddEdge(parameter_blocks[j], parameter_blocks[k]); + } + } + } + + return graph; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.h b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.h new file mode 100644 index 00000000000..1f9a4ff354f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.h @@ -0,0 +1,74 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Compute a parameter block ordering for use with the Schur +// complement based algorithms. + +#ifndef CERES_INTERNAL_SCHUR_ORDERING_H_ +#define CERES_INTERNAL_SCHUR_ORDERING_H_ + +#include <vector> +#include "ceres/graph.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class Program; +class ParameterBlock; + +// Uses an approximate independent set ordering to order the parameter +// blocks of a problem so that it is suitable for use with Schur +// complement based solvers. The output variable ordering contains an +// ordering of the parameter blocks and the return value is size of +// the independent set or the number of e_blocks (see +// schur_complement_solver.h for an explanation). Constant parameters +// are added to the end. +// +// The ordering vector has the structure +// +// ordering = [independent set, +// complement of the independent set, +// fixed blocks] +int ComputeSchurOrdering(const Program& program, + vector<ParameterBlock* >* ordering); + + +// Builds a graph on the parameter blocks of a Problem, whose +// structure reflects the sparsity structure of the Hessian. Each +// vertex corresponds to a parameter block in the Problem except for +// parameter blocks that are marked constant. An edge connects two +// parameter blocks, if they co-occur in a residual block. +Graph<ParameterBlock*>* CreateHessianGraph(const Program& program); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_ORDERING_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.cc b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.cc new file mode 100644 index 00000000000..6f0ceefd87d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.cc @@ -0,0 +1,78 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/scratch_evaluate_preparer.h" + +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" + +namespace ceres { +namespace internal { + +ScratchEvaluatePreparer* ScratchEvaluatePreparer::Create( + const Program &program, + int num_threads) { + ScratchEvaluatePreparer* preparers = new ScratchEvaluatePreparer[num_threads]; + int max_derivatives_per_residual_block = + program.MaxDerivativesPerResidualBlock(); + for (int i = 0; i < num_threads; i++) { + preparers[i].Init(max_derivatives_per_residual_block); + } + return preparers; +} + +void ScratchEvaluatePreparer::Init(int max_derivatives_per_residual_block) { + jacobian_scratch_.reset( + new double[max_derivatives_per_residual_block]); +} + +// Point the jacobian blocks into the scratch area of this evaluate preparer. +void ScratchEvaluatePreparer::Prepare(const ResidualBlock* residual_block, + int /* residual_block_index */, + SparseMatrix* /* jacobian */, + double** jacobians) { + double* jacobian_block_cursor = jacobian_scratch_.get(); + int num_residuals = residual_block->NumResiduals(); + int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (parameter_block->IsConstant()) { + jacobians[j] = NULL; + } else { + jacobians[j] = jacobian_block_cursor; + jacobian_block_cursor += num_residuals * parameter_block->LocalSize(); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.h b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.h new file mode 100644 index 00000000000..6b127081976 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.h @@ -0,0 +1,69 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A scratch evaluate preparer provides temporary storage for the jacobians that +// are created when running user-provided cost functions. The evaluator takes +// care to avoid evaluating the jacobian for fixed parameters. + +#ifndef CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ +#define CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ + +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +class Program; +class ResidualBlock; +class SparseMatrix; + +class ScratchEvaluatePreparer { + public: + // Create num_threads ScratchEvaluatePreparers. + static ScratchEvaluatePreparer* Create(const Program &program, + int num_threads); + + // EvaluatePreparer interface + void Init(int max_derivatives_per_residual_block); + void Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians); + + private: + // Scratch space for the jacobians; each jacobian is packed one after another. + // There is enough scratch to hold all the jacobians for the largest residual. + scoped_array<double> jacobian_scratch_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver.cc b/extern/libmv/third_party/ceres/internal/ceres/solver.cc new file mode 100644 index 00000000000..77f04d1d918 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/solver.cc @@ -0,0 +1,230 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/solver.h" + +#include <vector> +#include "ceres/levenberg_marquardt.h" +#include "ceres/program.h" +#include "ceres/solver_impl.h" +#include "ceres/stringprintf.h" +#include "ceres/problem.h" + +namespace ceres { + +Solver::~Solver() {} + +// TODO(sameeragarwal): The timing code here should use a sub-second +// timer. +void Solver::Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary) { + time_t start_time_seconds = time(NULL); + internal::SolverImpl::Solve(options, problem, summary); + summary->total_time_in_seconds = time(NULL) - start_time_seconds; + summary->preprocessor_time_in_seconds = + summary->total_time_in_seconds - summary->minimizer_time_in_seconds; +} + +void Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary) { + time_t start_time_seconds = time(NULL); + internal::SolverImpl::Solve(options, problem, summary); + summary->total_time_in_seconds = time(NULL) - start_time_seconds; + summary->preprocessor_time_in_seconds = + summary->total_time_in_seconds - summary->minimizer_time_in_seconds; +} + +Solver::Summary::Summary() + // Invalid values for most fields, to ensure that we are not + // accidentally reporting default values. + : termination_type(DID_NOT_RUN), + initial_cost(-1.0), + final_cost(-1.0), + fixed_cost(-1.0), + num_successful_steps(-1), + num_unsuccessful_steps(-1), + preprocessor_time_in_seconds(-1.0), + minimizer_time_in_seconds(-1.0), + total_time_in_seconds(-1.0), + num_parameter_blocks(-1), + num_parameters(-1), + num_residual_blocks(-1), + num_residuals(-1), + num_parameter_blocks_reduced(-1), + num_parameters_reduced(-1), + num_residual_blocks_reduced(-1), + num_residuals_reduced(-1), + num_eliminate_blocks_given(-1), + num_eliminate_blocks_used(-1), + num_threads_given(-1), + num_threads_used(-1), + num_linear_solver_threads_given(-1), + num_linear_solver_threads_used(-1), + linear_solver_type_given(SPARSE_NORMAL_CHOLESKY), + linear_solver_type_used(SPARSE_NORMAL_CHOLESKY), + preconditioner_type(IDENTITY), + ordering_type(NATURAL) { +} + +string Solver::Summary::BriefReport() const { + string report = "Ceres Solver Report: "; + if (termination_type == DID_NOT_RUN) { + CHECK(!error.empty()) + << "Solver terminated with DID_NOT_RUN but the solver did not " + << "return a reason. This is a Ceres error. Please report this " + << "to the Ceres team"; + return report + "Termination: DID_NOT_RUN, because " + error; + } + + internal::StringAppendF(&report, "Iterations: %d", + num_successful_steps + num_unsuccessful_steps); + internal::StringAppendF(&report, ", Initial cost: %e", initial_cost); + + // If the solver failed or was aborted, then the final_cost has no + // meaning. + if (termination_type != NUMERICAL_FAILURE && + termination_type != USER_ABORT) { + internal::StringAppendF(&report, ", Final cost: %e", final_cost); + } + + internal::StringAppendF(&report, ", Termination: %s.", + SolverTerminationTypeToString(termination_type)); + return report; +}; + +string Solver::Summary::FullReport() const { + string report = + "\n" + "Ceres Solver Report\n" + "-------------------\n"; + + if (termination_type == DID_NOT_RUN) { + internal::StringAppendF(&report, " Original\n"); + internal::StringAppendF(&report, "Parameter blocks % 10d\n", + num_parameter_blocks); + internal::StringAppendF(&report, "Parameters % 10d\n", + num_parameters); + internal::StringAppendF(&report, "Residual blocks % 10d\n", + num_residual_blocks); + internal::StringAppendF(&report, "Residual % 10d\n\n", + num_residuals); + } else { + internal::StringAppendF(&report, "%45s %21s\n", "Original", "Reduced"); + internal::StringAppendF(&report, "Parameter blocks % 25d% 25d\n", + num_parameter_blocks, num_parameter_blocks_reduced); + internal::StringAppendF(&report, "Parameters % 25d% 25d\n", + num_parameters, num_parameters_reduced); + internal::StringAppendF(&report, "Residual blocks % 25d% 25d\n", + num_residual_blocks, num_residual_blocks_reduced); + internal::StringAppendF(&report, "Residual % 25d% 25d\n\n", + num_residuals, num_residuals_reduced); + } + + internal::StringAppendF(&report, "%45s %21s\n", "Given", "Used"); + internal::StringAppendF(&report, "Linear solver %25s%25s\n", + LinearSolverTypeToString(linear_solver_type_given), + LinearSolverTypeToString(linear_solver_type_used)); + + if (linear_solver_type_given == CGNR || + linear_solver_type_given == ITERATIVE_SCHUR) { + internal::StringAppendF(&report, "Preconditioner %25s%25s\n", + PreconditionerTypeToString(preconditioner_type), + PreconditionerTypeToString(preconditioner_type)); + } else { + internal::StringAppendF(&report, "Preconditioner %25s%25s\n", + "N/A", "N/A"); + } + + internal::StringAppendF(&report, "Ordering %25s%25s\n", + OrderingTypeToString(ordering_type), + OrderingTypeToString(ordering_type)); + + if (IsSchurType(linear_solver_type_given)) { + if (ordering_type == SCHUR) { + internal::StringAppendF(&report, "num_eliminate_blocks%25s% 25d\n", + "N/A", + num_eliminate_blocks_used); + } else { + internal::StringAppendF(&report, "num_eliminate_blocks% 25d% 25d\n", + num_eliminate_blocks_given, + num_eliminate_blocks_used); + } + } + + internal::StringAppendF(&report, "Threads: % 25d% 25d\n", + num_threads_given, num_threads_used); + internal::StringAppendF(&report, "Linear Solver Threads:% 23d% 25d\n", + num_linear_solver_threads_given, + num_linear_solver_threads_used); + + + if (termination_type == DID_NOT_RUN) { + CHECK(!error.empty()) + << "Solver terminated with DID_NOT_RUN but the solver did not " + << "return a reason. This is a Ceres error. Please report this " + << "to the Ceres team"; + internal::StringAppendF(&report, "Termination: %20s\n", + "DID_NOT_RUN"); + internal::StringAppendF(&report, "Reason: %s\n", error.c_str()); + return report; + } + + internal::StringAppendF(&report, "\nCost:\n"); + internal::StringAppendF(&report, "Initial % 30e\n", initial_cost); + if (termination_type != NUMERICAL_FAILURE && termination_type != USER_ABORT) { + internal::StringAppendF(&report, "Final % 30e\n", final_cost); + internal::StringAppendF(&report, "Change % 30e\n", + initial_cost - final_cost); + } + + internal::StringAppendF(&report, "\nNumber of iterations:\n"); + internal::StringAppendF(&report, "Successful % 20d\n", + num_successful_steps); + internal::StringAppendF(&report, "Unsuccessful % 20d\n", + num_unsuccessful_steps); + internal::StringAppendF(&report, "Total % 20d\n", + num_successful_steps + num_unsuccessful_steps); + internal::StringAppendF(&report, "\nTime (in seconds):\n"); + internal::StringAppendF(&report, "Preprocessor % 25e\n", + preprocessor_time_in_seconds); + internal::StringAppendF(&report, "Minimizer % 25e\n", + minimizer_time_in_seconds); + internal::StringAppendF(&report, "Total % 25e\n", + total_time_in_seconds); + + internal::StringAppendF(&report, "Termination: %25s\n", + SolverTerminationTypeToString(termination_type)); + return report; +}; + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc new file mode 100644 index 00000000000..ed07d9dc6d7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc @@ -0,0 +1,693 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/solver_impl.h" + +#include <iostream> // NOLINT +#include <numeric> +#include "ceres/evaluator.h" +#include "ceres/gradient_checking_cost_function.h" +#include "ceres/levenberg_marquardt.h" +#include "ceres/linear_solver.h" +#include "ceres/map_util.h" +#include "ceres/minimizer.h" +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/schur_ordering.h" +#include "ceres/stringprintf.h" +#include "ceres/iteration_callback.h" +#include "ceres/problem.h" + +namespace ceres { +namespace internal { +namespace { + +void EvaluateCostAndResiduals(ProblemImpl* problem_impl, + double* cost, + vector<double>* residuals) { + CHECK_NOTNULL(cost); + Program* program = CHECK_NOTNULL(problem_impl)->mutable_program(); + if (residuals != NULL) { + residuals->resize(program->NumResiduals()); + program->Evaluate(cost, &(*residuals)[0]); + } else { + program->Evaluate(cost, NULL); + } +} + +// Callback for updating the user's parameter blocks. Updates are only +// done if the step is successful. +class StateUpdatingCallback : public IterationCallback { + public: + StateUpdatingCallback(Program* program, double* parameters) + : program_(program), parameters_(parameters) {} + + CallbackReturnType operator()(const IterationSummary& summary) { + if (summary.step_is_successful) { + program_->StateVectorToParameterBlocks(parameters_); + program_->CopyParameterBlockStateToUserState(); + } + return SOLVER_CONTINUE; + } + + private: + Program* program_; + double* parameters_; +}; + +// Callback for logging the state of the minimizer to STDERR or STDOUT +// depending on the user's preferences and logging level. +class LoggingCallback : public IterationCallback { + public: + explicit LoggingCallback(bool log_to_stdout) + : log_to_stdout_(log_to_stdout) {} + + ~LoggingCallback() {} + + CallbackReturnType operator()(const IterationSummary& summary) { + const char* kReportRowFormat = + "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " + "rho:% 3.2e mu:% 3.2e li:% 3d"; + string output = StringPrintf(kReportRowFormat, + summary.iteration, + summary.cost, + summary.cost_change, + summary.gradient_max_norm, + summary.step_norm, + summary.relative_decrease, + summary.mu, + summary.linear_solver_iterations); + if (log_to_stdout_) { + cout << output << endl; + } else { + VLOG(1) << output; + } + return SOLVER_CONTINUE; + } + + private: + const bool log_to_stdout_; +}; + +} // namespace + +void SolverImpl::Minimize(const Solver::Options& options, + Program* program, + Evaluator* evaluator, + LinearSolver* linear_solver, + double* initial_parameters, + double* final_parameters, + Solver::Summary* summary) { + Minimizer::Options minimizer_options(options); + + LoggingCallback logging_callback(options.minimizer_progress_to_stdout); + if (options.logging_type != SILENT) { + minimizer_options.callbacks.push_back(&logging_callback); + } + + StateUpdatingCallback updating_callback(program, initial_parameters); + if (options.update_state_every_iteration) { + minimizer_options.callbacks.push_back(&updating_callback); + } + + LevenbergMarquardt levenberg_marquardt; + + time_t start_minimizer_time_seconds = time(NULL); + levenberg_marquardt.Minimize(minimizer_options, + evaluator, + linear_solver, + initial_parameters, + final_parameters, + summary); + summary->minimizer_time_in_seconds = + time(NULL) - start_minimizer_time_seconds; +} + +void SolverImpl::Solve(const Solver::Options& original_options, + Problem* problem, + Solver::Summary* summary) { + Solver::Options options(original_options); + +#ifndef CERES_USE_OPENMP + if (options.num_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads=1 is supported. Switching" + << "to single threaded mode."; + options.num_threads = 1; + } + if (options.num_linear_solver_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_linear_solver_threads=1 is supported. Switching" + << "to single threaded mode."; + options.num_linear_solver_threads = 1; + } +#endif + + // Reset the summary object to its default values; + *CHECK_NOTNULL(summary) = Solver::Summary(); + summary->linear_solver_type_given = options.linear_solver_type; + summary->num_eliminate_blocks_given = original_options.num_eliminate_blocks; + summary->num_threads_given = original_options.num_threads; + summary->num_linear_solver_threads_given = + original_options.num_linear_solver_threads; + summary->ordering_type = original_options.ordering_type; + + ProblemImpl* problem_impl = CHECK_NOTNULL(problem)->problem_impl_.get(); + + summary->num_parameter_blocks = problem_impl->NumParameterBlocks(); + summary->num_parameters = problem_impl->NumParameters(); + summary->num_residual_blocks = problem_impl->NumResidualBlocks(); + summary->num_residuals = problem_impl->NumResiduals(); + + summary->num_threads_used = options.num_threads; + + // Evaluate the initial cost and residual vector (if needed). The + // initial cost needs to be computed on the original unpreprocessed + // problem, as it is used to determine the value of the "fixed" part + // of the objective function after the problem has undergone + // reduction. Also the initial residuals are in the order in which + // the user added the ResidualBlocks to the optimization problem. + EvaluateCostAndResiduals(problem_impl, + &summary->initial_cost, + options.return_initial_residuals + ? &summary->initial_residuals + : NULL); + + // If the user requests gradient checking, construct a new + // ProblemImpl by wrapping the CostFunctions of problem_impl inside + // GradientCheckingCostFunction and replacing problem_impl with + // gradient_checking_problem_impl. + scoped_ptr<ProblemImpl> gradient_checking_problem_impl; + if (options.check_gradients) { + VLOG(1) << "Checking Gradients"; + gradient_checking_problem_impl.reset( + CreateGradientCheckingProblemImpl( + problem_impl, + options.numeric_derivative_relative_step_size, + options.gradient_check_relative_precision)); + + // From here on, problem_impl will point to the GradientChecking version. + problem_impl = gradient_checking_problem_impl.get(); + } + + // Create the three objects needed to minimize: the transformed program, the + // evaluator, and the linear solver. + + scoped_ptr<Program> reduced_program( + CreateReducedProgram(&options, problem_impl, &summary->error)); + if (reduced_program == NULL) { + return; + } + + summary->num_parameter_blocks_reduced = reduced_program->NumParameterBlocks(); + summary->num_parameters_reduced = reduced_program->NumParameters(); + summary->num_residual_blocks_reduced = reduced_program->NumResidualBlocks(); + summary->num_residuals_reduced = reduced_program->NumResiduals(); + + scoped_ptr<LinearSolver> + linear_solver(CreateLinearSolver(&options, &summary->error)); + summary->linear_solver_type_used = options.linear_solver_type; + summary->preconditioner_type = options.preconditioner_type; + summary->num_eliminate_blocks_used = options.num_eliminate_blocks; + summary->num_linear_solver_threads_used = options.num_linear_solver_threads; + + if (linear_solver == NULL) { + return; + } + + if (!MaybeReorderResidualBlocks(options, + reduced_program.get(), + &summary->error)) { + return; + } + + scoped_ptr<Evaluator> evaluator( + CreateEvaluator(options, reduced_program.get(), &summary->error)); + if (evaluator == NULL) { + return; + } + + // The optimizer works on contiguous parameter vectors; allocate some. + Vector initial_parameters(reduced_program->NumParameters()); + Vector optimized_parameters(reduced_program->NumParameters()); + + // Collect the discontiguous parameters into a contiguous state vector. + reduced_program->ParameterBlocksToStateVector(&initial_parameters[0]); + + // Run the optimization. + Minimize(options, + reduced_program.get(), + evaluator.get(), + linear_solver.get(), + initial_parameters.data(), + optimized_parameters.data(), + summary); + + // If the user aborted mid-optimization or the optimization + // terminated because of a numerical failure, then return without + // updating user state. + if (summary->termination_type == USER_ABORT || + summary->termination_type == NUMERICAL_FAILURE) { + return; + } + + // Push the contiguous optimized parameters back to the user's parameters. + reduced_program->StateVectorToParameterBlocks(&optimized_parameters[0]); + reduced_program->CopyParameterBlockStateToUserState(); + + // Return the final cost and residuals for the original problem. + EvaluateCostAndResiduals(problem->problem_impl_.get(), + &summary->final_cost, + options.return_final_residuals + ? &summary->final_residuals + : NULL); + + // Stick a fork in it, we're done. + return; +} + +// Strips varying parameters and residuals, maintaining order, and updating +// num_eliminate_blocks. +bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program, + int* num_eliminate_blocks, + string* error) { + int original_num_eliminate_blocks = *num_eliminate_blocks; + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + + // Mark all the parameters as unused. Abuse the index member of the parameter + // blocks for the marking. + for (int i = 0; i < parameter_blocks->size(); ++i) { + (*parameter_blocks)[i]->set_index(-1); + } + + // Filter out residual that have all-constant parameters, and mark all the + // parameter blocks that appear in residuals. + { + vector<ResidualBlock*>* residual_blocks = + program->mutable_residual_blocks(); + int j = 0; + for (int i = 0; i < residual_blocks->size(); ++i) { + ResidualBlock* residual_block = (*residual_blocks)[i]; + int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Determine if the residual block is fixed, and also mark varying + // parameters that appear in the residual block. + bool all_constant = true; + for (int k = 0; k < num_parameter_blocks; k++) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[k]; + if (!parameter_block->IsConstant()) { + all_constant = false; + parameter_block->set_index(1); + } + } + + if (!all_constant) { + (*residual_blocks)[j++] = (*residual_blocks)[i]; + } + } + residual_blocks->resize(j); + } + + // Filter out unused or fixed parameter blocks, and update + // num_eliminate_blocks as necessary. + { + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + int j = 0; + for (int i = 0; i < parameter_blocks->size(); ++i) { + ParameterBlock* parameter_block = (*parameter_blocks)[i]; + if (parameter_block->index() == 1) { + (*parameter_blocks)[j++] = parameter_block; + } else if (i < original_num_eliminate_blocks) { + (*num_eliminate_blocks)--; + } + } + parameter_blocks->resize(j); + } + + CHECK(((program->NumResidualBlocks() == 0) && + (program->NumParameterBlocks() == 0)) || + ((program->NumResidualBlocks() != 0) && + (program->NumParameterBlocks() != 0))) + << "Congratulations, you found a bug in Ceres. Please report it."; + return true; +} + +Program* SolverImpl::CreateReducedProgram(Solver::Options* options, + ProblemImpl* problem_impl, + string* error) { + Program* original_program = problem_impl->mutable_program(); + scoped_ptr<Program> transformed_program(new Program(*original_program)); + + if (options->ordering_type == USER && + !ApplyUserOrdering(*problem_impl, + options->ordering, + transformed_program.get(), + error)) { + return NULL; + } + + if (options->ordering_type == SCHUR && options->num_eliminate_blocks != 0) { + *error = "Can't specify SCHUR ordering and num_eliminate_blocks " + "at the same time; SCHUR ordering determines " + "num_eliminate_blocks automatically."; + return NULL; + } + + if (options->ordering_type == SCHUR && options->ordering.size() != 0) { + *error = "Can't specify SCHUR ordering type and the ordering " + "vector at the same time; SCHUR ordering determines " + "a suitable parameter ordering automatically."; + return NULL; + } + + int num_eliminate_blocks = options->num_eliminate_blocks; + + if (!RemoveFixedBlocksFromProgram(transformed_program.get(), + &num_eliminate_blocks, + error)) { + return NULL; + } + + if (transformed_program->NumParameterBlocks() == 0) { + LOG(WARNING) << "No varying parameter blocks to optimize; " + << "bailing early."; + return transformed_program.release(); + } + + if (options->ordering_type == SCHUR) { + vector<ParameterBlock*> schur_ordering; + num_eliminate_blocks = ComputeSchurOrdering(*transformed_program, + &schur_ordering); + CHECK_EQ(schur_ordering.size(), transformed_program->NumParameterBlocks()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Replace the transformed program's ordering with the schur ordering. + swap(*transformed_program->mutable_parameter_blocks(), schur_ordering); + } + options->num_eliminate_blocks = num_eliminate_blocks; + CHECK_GE(options->num_eliminate_blocks, 0) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Since the transformed program is the "active" program, and it is mutated, + // update the parameter offsets and indices. + transformed_program->SetParameterOffsetsAndIndex(); + return transformed_program.release(); +} + +LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, + string* error) { +#ifdef CERES_NO_SUITESPARSE + if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY because SuiteSparse was not " + "enabled when Ceres was built."; + return NULL; + } +#endif // CERES_NO_SUITESPARSE + + if (options->linear_solver_max_num_iterations <= 0) { + *error = "Solver::Options::linear_solver_max_num_iterations is 0."; + return NULL; + } + if (options->linear_solver_min_num_iterations <= 0) { + *error = "Solver::Options::linear_solver_min_num_iterations is 0."; + return NULL; + } + if (options->linear_solver_min_num_iterations > + options->linear_solver_max_num_iterations) { + *error = "Solver::Options::linear_solver_min_num_iterations > " + "Solver::Options::linear_solver_max_num_iterations."; + return NULL; + } + + LinearSolver::Options linear_solver_options; + linear_solver_options.constant_sparsity = true; + linear_solver_options.min_num_iterations = + options->linear_solver_min_num_iterations; + linear_solver_options.max_num_iterations = + options->linear_solver_max_num_iterations; + linear_solver_options.type = options->linear_solver_type; + linear_solver_options.preconditioner_type = options->preconditioner_type; + +#ifdef CERES_NO_SUITESPARSE + if (linear_solver_options.preconditioner_type == SCHUR_JACOBI) { + *error = "SCHUR_JACOBI preconditioner not suppored. Please build Ceres " + "with SuiteSparse support"; + return NULL; + } + + if (linear_solver_options.preconditioner_type == CLUSTER_JACOBI) { + *error = "CLUSTER_JACOBI preconditioner not suppored. Please build Ceres " + "with SuiteSparse support"; + return NULL; + } + + if (linear_solver_options.preconditioner_type == CLUSTER_TRIDIAGONAL) { + *error = "CLUSTER_TRIDIAGONAL preconditioner not suppored. Please build " + "Ceres with SuiteSparse support"; + return NULL; + } +#endif + + linear_solver_options.num_threads = options->num_linear_solver_threads; + linear_solver_options.num_eliminate_blocks = + options->num_eliminate_blocks; + + if ((linear_solver_options.num_eliminate_blocks == 0) && + IsSchurType(linear_solver_options.type)) { +#ifndef CERES_NO_SUITESPARSE + LOG(INFO) << "No elimination block remaining " + << "switching to SPARSE_NORMAL_CHOLESKY."; + linear_solver_options.type = SPARSE_NORMAL_CHOLESKY; +#else + LOG(INFO) << "No elimination block remaining switching to DENSE_QR."; + linear_solver_options.type = DENSE_QR; +#endif // CERES_NO_SUITESPARSE + } + +#ifdef CERES_NO_SUITESPARSE + if (linear_solver_options.type == SPARSE_SCHUR) { + *error = "Can't use SPARSE_SCHUR because SuiteSparse was not " + "enabled when Ceres was built."; + return NULL; + } +#endif // CERES_NO_SUITESPARSE + + // The matrix used for storing the dense Schur complement has a + // single lock guarding the whole matrix. Running the + // SchurComplementSolver with multiple threads leads to maximum + // contention and slowdown. If the problem is large enough to + // benefit from a multithreaded schur eliminator, you should be + // using a SPARSE_SCHUR solver anyways. + if ((linear_solver_options.num_threads > 1) && + (linear_solver_options.type == DENSE_SCHUR)) { + LOG(WARNING) << "Warning: Solver::Options::num_linear_solver_threads = " + << options->num_linear_solver_threads + << " with DENSE_SCHUR will result in poor performance; " + << "switching to single-threaded."; + linear_solver_options.num_threads = 1; + } + + options->linear_solver_type = linear_solver_options.type; + options->num_linear_solver_threads = linear_solver_options.num_threads; + + return LinearSolver::Create(linear_solver_options); +} + +bool SolverImpl::ApplyUserOrdering(const ProblemImpl& problem_impl, + vector<double*>& ordering, + Program* program, + string* error) { + if (ordering.size() != program->NumParameterBlocks()) { + *error = StringPrintf("User specified ordering does not have the same " + "number of parameters as the problem. The problem" + "has %d blocks while the ordering has %ld blocks.", + program->NumParameterBlocks(), + ordering.size()); + return false; + } + + // Ensure that there are no duplicates in the user's ordering. + { + vector<double*> ordering_copy(ordering); + sort(ordering_copy.begin(), ordering_copy.end()); + if (unique(ordering_copy.begin(), ordering_copy.end()) + != ordering_copy.end()) { + *error = "User specified ordering contains duplicates."; + return false; + } + } + + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + + fill(parameter_blocks->begin(), + parameter_blocks->end(), + static_cast<ParameterBlock*>(NULL)); + + const ProblemImpl::ParameterMap& parameter_map = problem_impl.parameter_map(); + for (int i = 0; i < ordering.size(); ++i) { + ProblemImpl::ParameterMap::const_iterator it = + parameter_map.find(ordering[i]); + if (it == parameter_map.end()) { + *error = StringPrintf("User specified ordering contains a pointer " + "to a double that is not a parameter block in the " + "problem. The invalid double is at position %d " + " in options.ordering.", i); + return false; + } + (*parameter_blocks)[i] = it->second; + } + return true; +} + +// Find the minimum index of any parameter block to the given residual. +// Parameter blocks that have indices greater than num_eliminate_blocks are +// considered to have an index equal to num_eliminate_blocks. +int MinParameterBlock(const ResidualBlock* residual_block, + int num_eliminate_blocks) { + int min_parameter_block_position = num_eliminate_blocks; + for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[i]; + DCHECK_NE(parameter_block->index(), -1) + << "Did you forget to call Program::SetParameterOffsetsAndIndex()?"; + min_parameter_block_position = std::min(parameter_block->index(), + min_parameter_block_position); + } + return min_parameter_block_position; +} + +// Reorder the residuals for program, if necessary, so that the residuals +// involving each E block occur together. This is a necessary condition for the +// Schur eliminator, which works on these "row blocks" in the jacobian. +bool SolverImpl::MaybeReorderResidualBlocks(const Solver::Options& options, + Program* program, + string* error) { + // Only Schur types require the lexicographic reordering. + if (!IsSchurType(options.linear_solver_type)) { + return true; + } + + CHECK_NE(0, options.num_eliminate_blocks) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Create a histogram of the number of residuals for each E block. There is an + // extra bucket at the end to catch all non-eliminated F blocks. + vector<int> residual_blocks_per_e_block(options.num_eliminate_blocks + 1); + vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks(); + vector<int> min_position_per_residual(residual_blocks->size()); + for (int i = 0; i < residual_blocks->size(); ++i) { + ResidualBlock* residual_block = (*residual_blocks)[i]; + int position = MinParameterBlock(residual_block, + options.num_eliminate_blocks); + min_position_per_residual[i] = position; + DCHECK_LE(position, options.num_eliminate_blocks); + residual_blocks_per_e_block[position]++; + } + + // Run a cumulative sum on the histogram, to obtain offsets to the start of + // each histogram bucket (where each bucket is for the residuals for that + // E-block). + vector<int> offsets(options.num_eliminate_blocks + 1); + std::partial_sum(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end(), + offsets.begin()); + CHECK_EQ(offsets.back(), residual_blocks->size()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + CHECK(find(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end() - 1, 0) != + residual_blocks_per_e_block.end()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Fill in each bucket with the residual blocks for its corresponding E block. + // Each bucket is individually filled from the back of the bucket to the front + // of the bucket. The filling order among the buckets is dictated by the + // residual blocks. This loop uses the offsets as counters; subtracting one + // from each offset as a residual block is placed in the bucket. When the + // filling is finished, the offset pointerts should have shifted down one + // entry (this is verified below). + vector<ResidualBlock*> reordered_residual_blocks( + (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL)); + for (int i = 0; i < residual_blocks->size(); ++i) { + int bucket = min_position_per_residual[i]; + + // Decrement the cursor, which should now point at the next empty position. + offsets[bucket]--; + + // Sanity. + CHECK(reordered_residual_blocks[offsets[bucket]] == NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i]; + } + + // Sanity check #1: The difference in bucket offsets should match the + // histogram sizes. + for (int i = 0; i < options.num_eliminate_blocks; ++i) { + CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i]) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + // Sanity check #2: No NULL's left behind. + for (int i = 0; i < reordered_residual_blocks.size(); ++i) { + CHECK(reordered_residual_blocks[i] != NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + + // Now that the residuals are collected by E block, swap them in place. + swap(*program->mutable_residual_blocks(), reordered_residual_blocks); + return true; +} + +Evaluator* SolverImpl::CreateEvaluator(const Solver::Options& options, + Program* program, + string* error) { + Evaluator::Options evaluator_options; + evaluator_options.linear_solver_type = options.linear_solver_type; + evaluator_options.num_eliminate_blocks = options.num_eliminate_blocks; + evaluator_options.num_threads = options.num_threads; + return Evaluator::Create(evaluator_options, program, error); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h new file mode 100644 index 00000000000..957ebcc65df --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h @@ -0,0 +1,111 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_SOLVER_IMPL_H_ +#define CERES_INTERNAL_SOLVER_IMPL_H_ + +#include "ceres/solver.h" + +namespace ceres { +namespace internal { + +class Evaluator; +class LinearSolver; +class ProblemImpl; +class Program; + +class SolverImpl { + public: + // Mirrors the interface in solver.h, but exposes implementation + // details for testing internally. + static void Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary); + + // Create the transformed Program, which has all the fixed blocks + // and residuals eliminated, and in the case of automatic schur + // ordering, has the E blocks first in the resulting program, with + // options.num_eliminate_blocks set appropriately. + static Program* CreateReducedProgram(Solver::Options* options, + ProblemImpl* problem_impl, + string* error); + + // Create the appropriate linear solver, taking into account any + // config changes decided by CreateTransformedProgram(). The + // selected linear solver, which may be different from what the user + // selected; consider the case that the remaining elimininated + // blocks is zero after removing fixed blocks. + static LinearSolver* CreateLinearSolver(Solver::Options* options, + string* error); + + // Reorder the parameter blocks in program using the vector + // ordering. A return value of true indicates success and false + // indicates an error was encountered whose cause is logged to + // LOG(ERROR). + static bool ApplyUserOrdering(const ProblemImpl& problem_impl, + vector<double*>& ordering, + Program* program, + string* error); + + // Reorder the residuals for program, if necessary, so that the + // residuals involving each E block occur together. This is a + // necessary condition for the Schur eliminator, which works on + // these "row blocks" in the jacobian. + static bool MaybeReorderResidualBlocks(const Solver::Options& options, + Program* program, + string* error); + + // Create the appropriate evaluator for the transformed program. + static Evaluator* CreateEvaluator(const Solver::Options& options, + Program* program, + string* error); + + // Run the minimization for the given evaluator and configuration. + static void Minimize(const Solver::Options &options, + Program* program, + Evaluator* evaluator, + LinearSolver* linear_solver, + double* initial_parameters, + double* final_parameters, + Solver::Summary* summary); + + // Remove the fixed or unused parameter blocks and residuals + // depending only on fixed parameters from the problem. Also updates + // num_eliminate_blocks, since removed parameters changes the point + // at which the eliminated blocks is valid. + static bool RemoveFixedBlocksFromProgram(Program* program, + int* num_eliminate_blocks, + string* error); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SOLVER_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.cc new file mode 100644 index 00000000000..55336fd3130 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/sparse_matrix.h" + +namespace ceres { +namespace internal { + +SparseMatrix::~SparseMatrix() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h new file mode 100644 index 00000000000..562210dfec8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h @@ -0,0 +1,114 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Interface definition for sparse matrices. + +#ifndef CERES_INTERNAL_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_SPARSE_MATRIX_H_ + +#include <cstdio> +#include "ceres/linear_operator.h" +#include "ceres/internal/eigen.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; + +// This class defines the interface for storing and manipulating +// sparse matrices. The key property that differentiates different +// sparse matrices is how they are organized in memory and how the +// information about the sparsity structure of the matrix is +// stored. This has significant implications for linear solvers +// operating on these matrices. +// +// To deal with the different kinds of layouts, we will assume that a +// sparse matrix will have a two part representation. A values array +// that will be used to store the entries of the sparse matrix and +// some sort of a layout object that tells the user the sparsity +// structure and layout of the values array. For example in case of +// the TripletSparseMatrix, this information is carried in the rows +// and cols arrays and for the BlockSparseMatrix, this information is +// carried in the CompressedRowBlockStructure object. +// +// This interface deliberately does not contain any information about +// the structure of the sparse matrix as that seems to be highly +// matrix type dependent and we are at this stage unable to come up +// with an efficient high level interface that spans multiple sparse +// matrix types. +class SparseMatrix : public LinearOperator { + public: + virtual ~SparseMatrix(); + + // y += Ax; + virtual void RightMultiply(const double* x, double* y) const = 0; + // y += A'x; + virtual void LeftMultiply(const double* x, double* y) const = 0; + + // In MATLAB notation sum(A.*A, 1) + virtual void SquaredColumnNorm(double* x) const = 0; + // A = A * diag(scale) + virtual void ScaleColumns(const double* scale) = 0; + + // A = 0. A->num_nonzeros() == 0 is true after this call. The + // sparsity pattern is preserved. + virtual void SetZero() = 0; + + // Resize and populate dense_matrix with a dense version of the + // sparse matrix. + virtual void ToDenseMatrix(Matrix* dense_matrix) const = 0; + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + // Dump the sparse matrix to a proto. Destroys the contents of proto. + virtual void ToProto(SparseMatrixProto* proto) const = 0; +#endif + + // Write out the matrix as a sequence of (i,j,s) triplets. This + // format is useful for loading the matrix into MATLAB/octave as a + // sparse matrix. + virtual void ToTextFile(FILE* file) const = 0; + + // Accessors for the values array that stores the entries of the + // sparse matrix. The exact interpreptation of the values of this + // array depends on the particular kind of SparseMatrix being + // accessed. + virtual double* mutable_values() = 0; + virtual const double* values() const = 0; + + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; + virtual int num_nonzeros() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc new file mode 100644 index 00000000000..59222dc374d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_NO_SUITESPARSE + +#include "ceres/sparse_normal_cholesky_solver.h" + +#include <algorithm> +#include <cstring> +#include <ctime> +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/linear_solver.h" +#include "ceres/suitesparse.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +SparseNormalCholeskySolver::SparseNormalCholeskySolver( + const LinearSolver::Options& options) + : options_(options), symbolic_factor_(NULL) {} + +SparseNormalCholeskySolver::~SparseNormalCholeskySolver() { + if (symbolic_factor_ != NULL) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } +} + +LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double * x) { + const time_t start_time = time(NULL); + const int num_cols = A->num_cols(); + + LinearSolver::Summary summary; + Vector Atb = Vector::Zero(num_cols); + A->LeftMultiply(b, Atb.data()); + + if (per_solve_options.D != NULL) { + // Temporarily append a diagonal block to the A matrix, but undo it before + // returning the matrix to the user. + CompressedRowSparseMatrix D(per_solve_options.D, num_cols); + A->AppendRows(D); + } + + VectorRef(x, num_cols).setZero(); + + scoped_ptr<cholmod_sparse> lhs(ss_.CreateSparseMatrixTransposeView(A)); + CHECK_NOTNULL(lhs.get()); + + cholmod_dense* rhs = ss_.CreateDenseVector(Atb.data(), num_cols, num_cols); + const time_t init_time = time(NULL); + + if (symbolic_factor_ == NULL) { + symbolic_factor_ = CHECK_NOTNULL(ss_.AnalyzeCholesky(lhs.get())); + } + + const time_t symbolic_time = time(NULL); + + cholmod_dense* sol = ss_.SolveCholesky(lhs.get(), symbolic_factor_, rhs); + const time_t solve_time = time(NULL); + + ss_.Free(rhs); + rhs = NULL; + + if (per_solve_options.D != NULL) { + A->DeleteRows(num_cols); + } + + if (!options_.constant_sparsity) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } + + summary.num_iterations = 1; + if (sol != NULL) { + memcpy(x, sol->x, num_cols * sizeof(*x)); + + ss_.Free(sol); + sol = NULL; + summary.termination_type = TOLERANCE; + } + + const time_t cleanup_time = time(NULL); + VLOG(2) << "time (sec) total: " << cleanup_time - start_time + << " init: " << init_time - start_time + << " symbolic: " << symbolic_time - init_time + << " solve: " << solve_time - symbolic_time + << " cleanup: " << cleanup_time - solve_time; + return summary; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h new file mode 100644 index 00000000000..ce1d6d285be --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h @@ -0,0 +1,77 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A solver for sparse linear least squares problem based on solving +// the normal equations via a sparse cholesky factorization. + +#ifndef CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ +#define CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ + +#ifndef CERES_NO_SUITESPARSE + +#include "cholmod.h" +#include "cholmod_core.h" +#include "ceres/linear_solver.h" +#include "ceres/suitesparse.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; + +// Solves the normal equations (A'A + D'D) x = A'b, using the CHOLMOD sparse +// cholesky solver. +class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { + public: + explicit SparseNormalCholeskySolver(const LinearSolver::Options& options); + virtual ~SparseNormalCholeskySolver(); + + private: + virtual LinearSolver::Summary SolveImpl( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& options, + double* x); + + const LinearSolver::Options options_; + SuiteSparse ss_; + + // Cached factorization + cholmod_factor* symbolic_factor_; + DISALLOW_COPY_AND_ASSIGN(SparseNormalCholeskySolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE + +#endif // CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/split.cc b/extern/libmv/third_party/ceres/internal/ceres/split.cc new file mode 100644 index 00000000000..4fa1bd468b9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/split.cc @@ -0,0 +1,115 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include <string> +#include <vector> +#include <iterator> +#include "ceres/internal/port.h" + +namespace ceres { + +// If we know how much to allocate for a vector of strings, we can allocate the +// vector<string> only once and directly to the right size. This saves in +// between 33-66 % of memory space needed for the result, and runs faster in the +// microbenchmarks. +// +// The reserve is only implemented for the single character delim. +// +// The implementation for counting is cut-and-pasted from +// SplitStringToIteratorUsing. I could have written my own counting iterator, +// and use the existing template function, but probably this is more clear and +// more sure to get optimized to reasonable code. +static int CalculateReserveForVector(const string& full, const char* delim) { + int count = 0; + if (delim[0] != '\0' && delim[1] == '\0') { + // Optimize the common case where delim is a single character. + char c = delim[0]; + const char* p = full.data(); + const char* end = p + full.size(); + while (p != end) { + if (*p == c) { // This could be optimized with hasless(v,1) trick. + ++p; + } else { + while (++p != end && *p != c) { + // Skip to the next occurence of the delimiter. + } + ++count; + } + } + } + return count; +} + +template <typename StringType, typename ITR> +static inline +void SplitStringToIteratorUsing(const StringType& full, + const char* delim, + ITR& result) { + // Optimize the common case where delim is a single character. + if (delim[0] != '\0' && delim[1] == '\0') { + char c = delim[0]; + const char* p = full.data(); + const char* end = p + full.size(); + while (p != end) { + if (*p == c) { + ++p; + } else { + const char* start = p; + while (++p != end && *p != c) { + // Skip to the next occurence of the delimiter. + } + *result++ = StringType(start, p - start); + } + } + return; + } + + string::size_type begin_index, end_index; + begin_index = full.find_first_not_of(delim); + while (begin_index != string::npos) { + end_index = full.find_first_of(delim, begin_index); + if (end_index == string::npos) { + *result++ = full.substr(begin_index); + return; + } + *result++ = full.substr(begin_index, (end_index - begin_index)); + begin_index = full.find_first_not_of(delim, end_index); + } +} + +void SplitStringUsing(const string& full, + const char* delim, + vector<string>* result) { + result->reserve(result->size() + CalculateReserveForVector(full, delim)); + back_insert_iterator< vector<string> > it(*result); + SplitStringToIteratorUsing(full, delim, it); +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/stl_util.h b/extern/libmv/third_party/ceres/internal/ceres/stl_util.h new file mode 100644 index 00000000000..a1a19e8b3ce --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/stl_util.h @@ -0,0 +1,75 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_STL_UTIL_H_ +#define CERES_INTERNAL_STL_UTIL_H_ + +namespace ceres { + +// STLDeleteContainerPointers() +// For a range within a container of pointers, calls delete +// (non-array version) on these pointers. +// NOTE: for these three functions, we could just implement a DeleteObject +// functor and then call for_each() on the range and functor, but this +// requires us to pull in all of algorithm.h, which seems expensive. +// For hash_[multi]set, it is important that this deletes behind the iterator +// because the hash_set may call the hash function on the iterator when it is +// advanced, which could result in the hash function trying to deference a +// stale pointer. +template <class ForwardIterator> +void STLDeleteContainerPointers(ForwardIterator begin, + ForwardIterator end) { + while (begin != end) { + ForwardIterator temp = begin; + ++begin; + delete *temp; + } +} + +// STLDeleteElements() deletes all the elements in an STL container and clears +// the container. This function is suitable for use with a vector, set, +// hash_set, or any other STL container which defines sensible begin(), end(), +// and clear() methods. +// +// If container is NULL, this function is a no-op. +// +// As an alternative to calling STLDeleteElements() directly, consider +// ElementDeleter (defined below), which ensures that your container's elements +// are deleted when the ElementDeleter goes out of scope. +template <class T> +void STLDeleteElements(T *container) { + if (!container) return; + STLDeleteContainerPointers(container->begin(), container->end()); + container->clear(); +} + +} // namespace ceres + +#endif // CERES_INTERNAL_STL_UTIL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/stringprintf.cc b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.cc new file mode 100644 index 00000000000..c0f35225bc3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.cc @@ -0,0 +1,126 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Sanjay Ghemawat + +#include <cerrno> +#include <cstdarg> // For va_list and related operations +#include <cstdio> // MSVC requires this for _vsnprintf +#include <string> +#include <vector> + +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +#ifdef _MSC_VER +enum { IS_COMPILER_MSVC = 1 }; +#define va_copy(d,s) ((d) = (s)) +#else +enum { IS_COMPILER_MSVC = 0 }; +#endif + +void StringAppendV(string* dst, const char* format, va_list ap) { + // First try with a small fixed size buffer + char space[1024]; + + // It's possible for methods that use a va_list to invalidate + // the data in it upon use. The fix is to make a copy + // of the structure before using it and use that copy instead. + va_list backup_ap; + va_copy(backup_ap, ap); + int result = vsnprintf(space, sizeof(space), format, backup_ap); + va_end(backup_ap); + + if (result < sizeof(space)) { + if (result >= 0) { + // Normal case -- everything fit. + dst->append(space, result); + return; + } + + if (IS_COMPILER_MSVC) { + // Error or MSVC running out of space. MSVC 8.0 and higher + // can be asked about space needed with the special idiom below: + va_copy(backup_ap, ap); + result = vsnprintf(NULL, 0, format, backup_ap); + va_end(backup_ap); + } + + if (result < 0) { + // Just an error. + return; + } + } + + // Increase the buffer size to the size requested by vsnprintf, + // plus one for the closing \0. + int length = result+1; + char* buf = new char[length]; + + // Restore the va_list before we use it again + va_copy(backup_ap, ap); + result = vsnprintf(buf, length, format, backup_ap); + va_end(backup_ap); + + if (result >= 0 && result < length) { + // It fit + dst->append(buf, result); + } + delete[] buf; +} + + +string StringPrintf(const char* format, ...) { + va_list ap; + va_start(ap, format); + string result; + StringAppendV(&result, format, ap); + va_end(ap); + return result; +} + +const string& SStringPrintf(string* dst, const char* format, ...) { + va_list ap; + va_start(ap, format); + dst->clear(); + StringAppendV(dst, format, ap); + va_end(ap); + return *dst; +} + +void StringAppendF(string* dst, const char* format, ...) { + va_list ap; + va_start(ap, format); + StringAppendV(dst, format, ap); + va_end(ap); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h new file mode 100644 index 00000000000..30b974e7ae5 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h @@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Sanjay Ghemawat +// +// Printf variants that place their output in a C++ string. +// +// Usage: +// string result = StringPrintf("%d %s\n", 10, "hello"); +// SStringPrintf(&result, "%d %s\n", 10, "hello"); +// StringAppendF(&result, "%d %s\n", 20, "there"); + +#ifndef CERES_INTERNAL_STRINGPRINTF_H_ +#define CERES_INTERNAL_STRINGPRINTF_H_ + +#include <cstdarg> +#include <string> + +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +#if (defined(__GNUC__) || defined(__clang__)) +// Tell the compiler to do printf format string checking if the compiler +// supports it; see the 'format' attribute in +// <http://gcc.gnu.org/onlinedocs/gcc-4.3.0/gcc/Function-Attributes.html>. +// +// N.B.: As the GCC manual states, "[s]ince non-static C++ methods +// have an implicit 'this' argument, the arguments of such methods +// should be counted from two, not one." +#define PRINTF_ATTRIBUTE(string_index, first_to_check) \ + __attribute__((__format__ (__printf__, string_index, first_to_check))) +#define SCANF_ATTRIBUTE(string_index, first_to_check) \ + __attribute__((__format__ (__scanf__, string_index, first_to_check))) +#else +#define PRINTF_ATTRIBUTE(string_index, first_to_check) +#endif + +// Return a C++ string. +extern string StringPrintf(const char* format, ...) + // Tell the compiler to do printf format string checking. + PRINTF_ATTRIBUTE(1,2); + +// Store result into a supplied string and return it. +extern const string& SStringPrintf(string* dst, const char* format, ...) + // Tell the compiler to do printf format string checking. + PRINTF_ATTRIBUTE(2,3); + +// Append result to a supplied string. +extern void StringAppendF(string* dst, const char* format, ...) + // Tell the compiler to do printf format string checking. + PRINTF_ATTRIBUTE(2,3); + +// Lower-level routine that takes a va_list and appends to a specified string. +// All other routines are just convenience wrappers around it. +extern void StringAppendV(string* dst, const char* format, va_list ap); + +#undef PRINTF_ATTRIBUTE + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_STRINGPRINTF_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc new file mode 100644 index 00000000000..1cf6a7496a7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc @@ -0,0 +1,193 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_NO_SUITESPARSE + +#include "ceres/suitesparse.h" + +#include "cholmod.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +namespace ceres { +namespace internal { + +cholmod_sparse* SuiteSparse::CreateSparseMatrix(TripletSparseMatrix* A) { + cholmod_triplet triplet; + + triplet.nrow = A->num_rows(); + triplet.ncol = A->num_cols(); + triplet.nzmax = A->max_num_nonzeros(); + triplet.nnz = A->num_nonzeros(); + triplet.i = reinterpret_cast<void*>(A->mutable_rows()); + triplet.j = reinterpret_cast<void*>(A->mutable_cols()); + triplet.x = reinterpret_cast<void*>(A->mutable_values()); + triplet.stype = 0; // Matrix is not symmetric. + triplet.itype = CHOLMOD_INT; + triplet.xtype = CHOLMOD_REAL; + triplet.dtype = CHOLMOD_DOUBLE; + + return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_); +} + + +cholmod_sparse* SuiteSparse::CreateSparseMatrixTranspose( + TripletSparseMatrix* A) { + cholmod_triplet triplet; + + triplet.ncol = A->num_rows(); // swap row and columns + triplet.nrow = A->num_cols(); + triplet.nzmax = A->max_num_nonzeros(); + triplet.nnz = A->num_nonzeros(); + + // swap rows and columns + triplet.j = reinterpret_cast<void*>(A->mutable_rows()); + triplet.i = reinterpret_cast<void*>(A->mutable_cols()); + triplet.x = reinterpret_cast<void*>(A->mutable_values()); + triplet.stype = 0; // Matrix is not symmetric. + triplet.itype = CHOLMOD_INT; + triplet.xtype = CHOLMOD_REAL; + triplet.dtype = CHOLMOD_DOUBLE; + + return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_); +} + +cholmod_sparse* SuiteSparse::CreateSparseMatrixTransposeView( + CompressedRowSparseMatrix* A) { + cholmod_sparse* m = new cholmod_sparse_struct; + m->nrow = A->num_cols(); + m->ncol = A->num_rows(); + m->nzmax = A->num_nonzeros(); + + m->p = reinterpret_cast<void*>(A->mutable_rows()); + m->i = reinterpret_cast<void*>(A->mutable_cols()); + m->x = reinterpret_cast<void*>(A->mutable_values()); + + m->stype = 0; // Matrix is not symmetric. + m->itype = CHOLMOD_INT; + m->xtype = CHOLMOD_REAL; + m->dtype = CHOLMOD_DOUBLE; + m->sorted = 1; + m->packed = 1; + + return m; +} + +cholmod_dense* SuiteSparse::CreateDenseVector(const double* x, + int in_size, + int out_size) { + CHECK_LE(in_size, out_size); + cholmod_dense* v = cholmod_zeros(out_size, 1, CHOLMOD_REAL, &cc_); + if (x != NULL) { + memcpy(v->x, x, in_size*sizeof(*x)); + } + return v; +} + +cholmod_factor* SuiteSparse::AnalyzeCholesky(cholmod_sparse* A) { + cholmod_factor* factor = cholmod_analyze(A, &cc_); + CHECK_EQ(cc_.status, CHOLMOD_OK) + << "Cholmod symbolic analysis failed " << cc_.status; + CHECK_NOTNULL(factor); + return factor; +} + +bool SuiteSparse::Cholesky(cholmod_sparse* A, cholmod_factor* L) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(L); + + cc_.quick_return_if_not_posdef = 1; + int status = cholmod_factorize(A, L, &cc_); + switch (cc_.status) { + case CHOLMOD_NOT_INSTALLED: + LOG(WARNING) << "Cholmod failure: method not installed."; + return false; + case CHOLMOD_OUT_OF_MEMORY: + LOG(WARNING) << "Cholmod failure: out of memory."; + return false; + case CHOLMOD_TOO_LARGE: + LOG(WARNING) << "Cholmod failure: integer overflow occured."; + return false; + case CHOLMOD_INVALID: + LOG(WARNING) << "Cholmod failure: invalid input."; + return false; + case CHOLMOD_NOT_POSDEF: + // TODO(sameeragarwal): These two warnings require more + // sophisticated handling going forward. For now we will be + // strict and treat them as failures. + LOG(WARNING) << "Cholmod warning: matrix not positive definite."; + return false; + case CHOLMOD_DSMALL: + LOG(WARNING) << "Cholmod warning: D for LDL' or diag(L) or " + << "LL' has tiny absolute value."; + return false; + case CHOLMOD_OK: + if (status != 0) { + return true; + } + LOG(WARNING) << "Cholmod failure: cholmod_factorize returned zero " + << "but cholmod_common::status is CHOLMOD_OK." + << "Please report this to ceres-solver@googlegroups.com."; + return false; + default: + LOG(WARNING) << "Unknown cholmod return code. " + << "Please report this to ceres-solver@googlegroups.com."; + return false; + } + return false; +} + +cholmod_dense* SuiteSparse::Solve(cholmod_factor* L, + cholmod_dense* b) { + if (cc_.status != CHOLMOD_OK) { + LOG(WARNING) << "CHOLMOD status NOT OK"; + return NULL; + } + + return cholmod_solve(CHOLMOD_A, L, b, &cc_); +} + +cholmod_dense* SuiteSparse::SolveCholesky(cholmod_sparse* A, + cholmod_factor* L, + cholmod_dense* b) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(L); + CHECK_NOTNULL(b); + + if (Cholesky(A, L)) { + return Solve(L, b); + } + + return NULL; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h new file mode 100644 index 00000000000..091e67a69a9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h @@ -0,0 +1,159 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A simple C++ interface to the SuiteSparse and CHOLMOD libraries. + +#ifndef CERES_INTERNAL_SUITESPARSE_H_ +#define CERES_INTERNAL_SUITESPARSE_H_ + +#ifndef CERES_NO_SUITESPARSE + +#include <cstring> +#include <string> + +#include <glog/logging.h> +#include "cholmod.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; +class TripletSparseMatrix; + +// The raw CHOLMOD and SuiteSparseQR libraries have a slightly +// cumbersome c like calling format. This object abstracts it away and +// provides the user with a simpler interface. The methods here cannot +// be static as a cholmod_common object serves as a global variable +// for all cholmod function calls. +class SuiteSparse { + public: + SuiteSparse() { cholmod_start(&cc_); } + ~SuiteSparse() { cholmod_finish(&cc_); } + + // Functions for building cholmod_sparse objects from sparse + // matrices stored in triplet form. The matrix A is not + // modifed. Called owns the result. + cholmod_sparse* CreateSparseMatrix(TripletSparseMatrix* A); + + // This function works like CreateSparseMatrix, except that the + // return value corresponds to A' rather than A. + cholmod_sparse* CreateSparseMatrixTranspose(TripletSparseMatrix* A); + + // Create a cholmod_sparse wrapper around the contents of A. This is + // a shallow object, which refers to the contents of A and does not + // use the SuiteSparse machinery to allocate memory, this object + // should be disposed off with a delete and not a call to Free as is + // the case for objects returned by CreateSparseMatrixTranspose. + cholmod_sparse* CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A); + + // Given a vector x, build a cholmod_dense vector of size out_size + // with the first in_size entries copied from x. If x is NULL, then + // an all zeros vector is returned. Caller owns the result. + cholmod_dense* CreateDenseVector(const double* x, int in_size, int out_size); + + // The matrix A is scaled using the matrix whose diagonal is the + // vector scale. mode describes how scaling is applied. Possible + // values are CHOLMOD_ROW for row scaling - diag(scale) * A, + // CHOLMOD_COL for column scaling - A * diag(scale) and CHOLMOD_SYM + // for symmetric scaling which scales both the rows and the columns + // - diag(scale) * A * diag(scale). + void Scale(cholmod_dense* scale, int mode, cholmod_sparse* A) { + cholmod_scale(scale, mode, A, &cc_); + } + + // Create and return a matrix m = A * A'. Caller owns the + // result. The matrix A is not modified. + cholmod_sparse* AATranspose(cholmod_sparse* A) { + cholmod_sparse*m = cholmod_aat(A, NULL, A->nrow, 1, &cc_); + m->stype = 1; // Pay attention to the upper triangular part. + return m; + } + + // y = alpha * A * x + beta * y. Only y is modified. + void SparseDenseMultiply(cholmod_sparse* A, double alpha, double beta, + cholmod_dense* x, cholmod_dense* y) { + double alpha_[2] = {alpha, 0}; + double beta_[2] = {beta, 0}; + cholmod_sdmult(A, 0, alpha_, beta_, x, y, &cc_); + } + + // Analyze the sparsity structure of the matrix A compute the + // symbolic factorization of A. A is not modified, only the pattern + // of non-zeros of A is used, the actual numerical values in A are + // of no consequence. Caller owns the result. + cholmod_factor* AnalyzeCholesky(cholmod_sparse* A); + + // Use the symbolic factorization in L, to find the numerical + // factorization for the matrix A or AA^T. Return true if + // successful, false otherwise. L contains the numeric factorization + // on return. + bool Cholesky(cholmod_sparse* A, cholmod_factor* L); + + // Given a Cholesky factorization of a matrix A = LL^T, solve the + // linear system Ax = b, and return the result. If the Solve fails + // NULL is returned. Caller owns the result. + cholmod_dense* Solve(cholmod_factor* L, cholmod_dense* b); + + // Combine the calls to Cholesky and Solve into a single call. If + // the cholesky factorization or the solve fails, return + // NULL. Caller owns the result. + cholmod_dense* SolveCholesky(cholmod_sparse* A, + cholmod_factor* L, + cholmod_dense* b); + + void Free(cholmod_sparse* m) { cholmod_free_sparse(&m, &cc_); } + void Free(cholmod_dense* m) { cholmod_free_dense(&m, &cc_); } + void Free(cholmod_factor* m) { cholmod_free_factor(&m, &cc_); } + + void Print(cholmod_sparse* m, const string& name) { + cholmod_print_sparse(m, const_cast<char*>(name.c_str()), &cc_); + } + + void Print(cholmod_dense* m, const string& name) { + cholmod_print_dense(m, const_cast<char*>(name.c_str()), &cc_); + } + + void Print(cholmod_triplet* m, const string& name) { + cholmod_print_triplet(m, const_cast<char*>(name.c_str()), &cc_); + } + + cholmod_common* mutable_cc() { return &cc_; } + + private: + cholmod_common cc_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE + +#endif // CERES_INTERNAL_SUITESPARSE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc new file mode 100644 index 00000000000..247ab2e697b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc @@ -0,0 +1,306 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/triplet_sparse_matrix.h" + +#include <algorithm> +#include <cstddef> +#include <glog/logging.h> +#include "ceres/matrix_proto.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +TripletSparseMatrix::TripletSparseMatrix() + : num_rows_(0), + num_cols_(0), + max_num_nonzeros_(0), + num_nonzeros_(0), + rows_(NULL), + cols_(NULL), + values_(NULL) {} + +TripletSparseMatrix::~TripletSparseMatrix() {} + +TripletSparseMatrix::TripletSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros) + : num_rows_(num_rows), + num_cols_(num_cols), + max_num_nonzeros_(max_num_nonzeros), + num_nonzeros_(0), + rows_(NULL), + cols_(NULL), + values_(NULL) { + // All the sizes should at least be zero + CHECK_GE(num_rows, 0); + CHECK_GE(num_cols, 0); + CHECK_GE(max_num_nonzeros, 0); + AllocateMemory(); +} + +TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig) + : num_rows_(orig.num_rows_), + num_cols_(orig.num_cols_), + max_num_nonzeros_(orig.max_num_nonzeros_), + num_nonzeros_(orig.num_nonzeros_), + rows_(NULL), + cols_(NULL), + values_(NULL) { + AllocateMemory(); + CopyData(orig); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +TripletSparseMatrix::TripletSparseMatrix(const SparseMatrixProto& outer_proto) { + CHECK(outer_proto.has_triplet_matrix()); + + const TripletSparseMatrixProto& proto = outer_proto.triplet_matrix(); + CHECK(proto.has_num_rows()); + CHECK(proto.has_num_cols()); + CHECK_EQ(proto.rows_size(), proto.cols_size()); + CHECK_EQ(proto.cols_size(), proto.values_size()); + + // Initialize the matrix with the appropriate size and capacity. + max_num_nonzeros_ = 0; + set_num_nonzeros(0); + Reserve(proto.num_nonzeros()); + Resize(proto.num_rows(), proto.num_cols()); + set_num_nonzeros(proto.num_nonzeros()); + + // Copy the entries in. + for (int i = 0; i < proto.num_nonzeros(); ++i) { + rows_[i] = proto.rows(i); + cols_[i] = proto.cols(i); + values_[i] = proto.values(i); + } +} +#endif + +TripletSparseMatrix& TripletSparseMatrix::operator=( + const TripletSparseMatrix& rhs) { + num_rows_ = rhs.num_rows_; + num_cols_ = rhs.num_cols_; + num_nonzeros_ = rhs.num_nonzeros_; + max_num_nonzeros_ = rhs.max_num_nonzeros_; + AllocateMemory(); + CopyData(rhs); + return *this; +} + +bool TripletSparseMatrix::AllTripletsWithinBounds() const { + for (int i = 0; i < num_nonzeros_; ++i) { + if ((rows_[i] < 0) || (rows_[i] >= num_rows_) || + (cols_[i] < 0) || (cols_[i] >= num_cols_)) + return false; + } + return true; +} + +void TripletSparseMatrix::Reserve(int new_max_num_nonzeros) { + CHECK_LE(num_nonzeros_, new_max_num_nonzeros) + << "Reallocation will cause data loss"; + + // Nothing to do if we have enough space already. + if (new_max_num_nonzeros <= max_num_nonzeros_) + return; + + int* new_rows = new int[new_max_num_nonzeros]; + int* new_cols = new int[new_max_num_nonzeros]; + double* new_values = new double[new_max_num_nonzeros]; + + for (int i = 0; i < num_nonzeros_; ++i) { + new_rows[i] = rows_[i]; + new_cols[i] = cols_[i]; + new_values[i] = values_[i]; + } + + rows_.reset(new_rows); + cols_.reset(new_cols); + values_.reset(new_values); + + max_num_nonzeros_ = new_max_num_nonzeros; +} + +void TripletSparseMatrix::SetZero() { + fill(values_.get(), values_.get() + max_num_nonzeros_, 0.0); + num_nonzeros_ = 0; +} + +void TripletSparseMatrix::set_num_nonzeros(int num_nonzeros) { + CHECK_GE(num_nonzeros, 0); + CHECK_LE(num_nonzeros, max_num_nonzeros_); + num_nonzeros_ = num_nonzeros; +}; + +void TripletSparseMatrix::AllocateMemory() { + rows_.reset(new int[max_num_nonzeros_]); + cols_.reset(new int[max_num_nonzeros_]); + values_.reset(new double[max_num_nonzeros_]); +} + +void TripletSparseMatrix::CopyData(const TripletSparseMatrix& orig) { + for (int i = 0; i < num_nonzeros_; ++i) { + rows_[i] = orig.rows_[i]; + cols_[i] = orig.cols_[i]; + values_[i] = orig.values_[i]; + } +} + +void TripletSparseMatrix::RightMultiply(const double* x, double* y) const { + for (int i = 0; i < num_nonzeros_; ++i) { + y[rows_[i]] += values_[i]*x[cols_[i]]; + } +} + +void TripletSparseMatrix::LeftMultiply(const double* x, double* y) const { + for (int i = 0; i < num_nonzeros_; ++i) { + y[cols_[i]] += values_[i]*x[rows_[i]]; + } +} + +void TripletSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + VectorRef(x, num_cols_).setZero(); + for (int i = 0; i < num_nonzeros_; ++i) { + x[cols_[i]] += values_[i] * values_[i]; + } +} + +void TripletSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + for (int i = 0; i < num_nonzeros_; ++i) { + values_[i] = values_[i] * scale[cols_[i]]; + } +} + +void TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + Matrix& m = *dense_matrix; + for (int i = 0; i < num_nonzeros_; ++i) { + m(rows_[i], cols_[i]) += values_[i]; + } +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void TripletSparseMatrix::ToProto(SparseMatrixProto *proto) const { + proto->Clear(); + + TripletSparseMatrixProto* tsm_proto = proto->mutable_triplet_matrix(); + tsm_proto->set_num_rows(num_rows_); + tsm_proto->set_num_cols(num_cols_); + tsm_proto->set_num_nonzeros(num_nonzeros_); + for (int i = 0; i < num_nonzeros_; ++i) { + tsm_proto->add_rows(rows_[i]); + tsm_proto->add_cols(cols_[i]); + tsm_proto->add_values(values_[i]); + } +} +#endif + +void TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) { + CHECK_EQ(B.num_cols(), num_cols_); + Reserve(num_nonzeros_ + B.num_nonzeros_); + for (int i = 0; i < B.num_nonzeros_; ++i) { + rows_.get()[num_nonzeros_] = B.rows()[i] + num_rows_; + cols_.get()[num_nonzeros_] = B.cols()[i]; + values_.get()[num_nonzeros_++] = B.values()[i]; + } + num_rows_ = num_rows_ + B.num_rows(); +} + +void TripletSparseMatrix::AppendCols(const TripletSparseMatrix& B) { + CHECK_EQ(B.num_rows(), num_rows_); + Reserve(num_nonzeros_ + B.num_nonzeros_); + for (int i = 0; i < B.num_nonzeros_; ++i, ++num_nonzeros_) { + rows_.get()[num_nonzeros_] = B.rows()[i]; + cols_.get()[num_nonzeros_] = B.cols()[i] + num_cols_; + values_.get()[num_nonzeros_] = B.values()[i]; + } + num_cols_ = num_cols_ + B.num_cols(); +} + + +void TripletSparseMatrix::Resize(int new_num_rows, int new_num_cols) { + if ((new_num_rows >= num_rows_) && (new_num_cols >= num_cols_)) { + num_rows_ = new_num_rows; + num_cols_ = new_num_cols; + return; + } + + num_rows_ = new_num_rows; + num_cols_ = new_num_cols; + + int* r_ptr = rows_.get(); + int* c_ptr = cols_.get(); + double* v_ptr = values_.get(); + + int dropped_terms = 0; + for (int i = 0; i < num_nonzeros_; ++i) { + if ((r_ptr[i] < num_rows_) && (c_ptr[i] < num_cols_)) { + if (dropped_terms) { + r_ptr[i-dropped_terms] = r_ptr[i]; + c_ptr[i-dropped_terms] = c_ptr[i]; + v_ptr[i-dropped_terms] = v_ptr[i]; + } + } else { + ++dropped_terms; + } + } + num_nonzeros_ -= dropped_terms; +} + +TripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix( + const double* values, int num_rows) { + TripletSparseMatrix* m = + new TripletSparseMatrix(num_rows, num_rows, num_rows); + for (int i = 0; i < num_rows; ++i) { + m->mutable_rows()[i] = i; + m->mutable_cols()[i] = i; + m->mutable_values()[i] = values[i]; + } + m->set_num_nonzeros(num_rows); + return m; +} + +void TripletSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + for (int i = 0; i < num_nonzeros_; ++i) { + fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h new file mode 100644 index 00000000000..300e74d0bbc --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h @@ -0,0 +1,137 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H_ + +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; + +// An implementation of the SparseMatrix interface to store and +// manipulate sparse matrices in triplet (i,j,s) form. This object is +// inspired by the design of the cholmod_triplet struct used in the +// SuiteSparse package and is memory layout compatible with it. +class TripletSparseMatrix : public SparseMatrix { + public: + TripletSparseMatrix(); + TripletSparseMatrix(int num_rows, int num_cols, int max_num_nonzeros); + explicit TripletSparseMatrix(const TripletSparseMatrix& orig); +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit TripletSparseMatrix(const SparseMatrixProto& proto); +#endif + + TripletSparseMatrix& operator=(const TripletSparseMatrix& rhs); + + ~TripletSparseMatrix(); + + // Implementation of the SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto *proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_cols_; } + virtual int num_nonzeros() const { return num_nonzeros_; } + virtual const double* values() const { return values_.get(); } + virtual double* mutable_values() { return values_.get(); } + virtual void set_num_nonzeros(int num_nonzeros); + + // Increase max_num_nonzeros and correspondingly increase the size + // of rows_, cols_ and values_. If new_max_num_nonzeros is smaller + // than max_num_nonzeros_, then num_non_zeros should be less than or + // equal to new_max_num_nonzeros, otherwise data loss is possible + // and the method crashes. + void Reserve(int new_max_num_nonzeros); + + // Append the matrix B at the bottom of this matrix. B should have + // the same number of columns as num_cols_. + void AppendRows(const TripletSparseMatrix& B); + + // Append the matrix B at the right of this matrix. B should have + // the same number of rows as num_rows_; + void AppendCols(const TripletSparseMatrix& B); + + // Resize the matrix. Entries which fall outside the new matrix + // bounds are dropped and the num_non_zeros changed accordingly. + void Resize(int new_num_rows, int new_num_cols); + + int max_num_nonzeros() const { return max_num_nonzeros_; } + const int* rows() const { return rows_.get(); } + const int* cols() const { return cols_.get(); } + int* mutable_rows() { return rows_.get(); } + int* mutable_cols() { return cols_.get(); } + + // Returns true if the entries of the matrix obey the row, column, + // and column size bounds and false otherwise. + bool AllTripletsWithinBounds() const; + + bool IsValid() const { return AllTripletsWithinBounds(); } + + // Build a sparse diagonal matrix of size num_rows x num_rows from + // the array values. Entries of the values array are copied into the + // sparse matrix. + static TripletSparseMatrix* CreateSparseDiagonalMatrix(const double* values, + int num_rows); + + private: + void AllocateMemory(); + void CopyData(const TripletSparseMatrix& orig); + + int num_rows_; + int num_cols_; + int max_num_nonzeros_; + int num_nonzeros_; + + // The data is stored as three arrays. For each i, values_[i] is + // stored at the location (rows_[i], cols_[i]). If the there are + // multiple entries with the same (rows_[i], cols_[i]), the values_ + // entries corresponding to them are summed up. + scoped_array<int> rows_; + scoped_array<int> cols_; + scoped_array<double> values_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H__ diff --git a/extern/libmv/third_party/ceres/internal/ceres/types.cc b/extern/libmv/third_party/ceres/internal/ceres/types.cc new file mode 100644 index 00000000000..860f8a43f37 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/types.cc @@ -0,0 +1,98 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <string> +#include "ceres/types.h" + +namespace ceres { + +#define CASESTR(x) case x: return #x + +const char* LinearSolverTypeToString(LinearSolverType solver_type) { + switch (solver_type) { + CASESTR(SPARSE_NORMAL_CHOLESKY); + CASESTR(DENSE_QR); + CASESTR(DENSE_SCHUR); + CASESTR(SPARSE_SCHUR); + CASESTR(ITERATIVE_SCHUR); + CASESTR(CGNR); + default: + return "UNKNOWN"; + } +} + +const char* PreconditionerTypeToString( + PreconditionerType preconditioner_type) { + switch (preconditioner_type) { + CASESTR(IDENTITY); + CASESTR(JACOBI); + CASESTR(SCHUR_JACOBI); + CASESTR(CLUSTER_JACOBI); + CASESTR(CLUSTER_TRIDIAGONAL); + default: + return "UNKNOWN"; + } +} + +const char* OrderingTypeToString(OrderingType ordering_type) { + switch (ordering_type) { + CASESTR(NATURAL); + CASESTR(USER); + CASESTR(SCHUR); + default: + return "UNKNOWN"; + } +} + +const char* SolverTerminationTypeToString( + SolverTerminationType termination_type) { + switch (termination_type) { + CASESTR(NO_CONVERGENCE); + CASESTR(FUNCTION_TOLERANCE); + CASESTR(GRADIENT_TOLERANCE); + CASESTR(PARAMETER_TOLERANCE); + CASESTR(NUMERICAL_FAILURE); + CASESTR(USER_ABORT); + CASESTR(USER_SUCCESS); + CASESTR(DID_NOT_RUN); + default: + return "UNKNOWN"; + } +} + +#undef CASESTR + +bool IsSchurType(LinearSolverType type) { + return ((type == SPARSE_SCHUR) || + (type == DENSE_SCHUR) || + (type == ITERATIVE_SCHUR)); +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility.cc b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc new file mode 100644 index 00000000000..fd41648a7af --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc @@ -0,0 +1,150 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: kushalav@google.com (Avanish Kushal) + +#include <cmath> +#include <ctime> +#include <algorithm> +#include <set> +#include <vector> +#include <utility> + +#include <glog/logging.h> +#include "ceres/block_structure.h" +#include "ceres/collections_port.h" +#include "ceres/graph.h" + +namespace ceres { +namespace internal { + +void ComputeVisibility(const CompressedRowBlockStructure& block_structure, + const int num_eliminate_blocks, + vector< set<int> >* visibility) { + CHECK_NOTNULL(visibility); + + // Clear the visibility vector and resize it to hold a + // vector for each camera. + visibility->resize(0); + visibility->resize(block_structure.cols.size() - num_eliminate_blocks); + + for (int i = 0; i < block_structure.rows.size(); ++i) { + const vector<Cell>& cells = block_structure.rows[i].cells; + int block_id = cells[0].block_id; + // If the first block is not an e_block, then skip this row block. + if (block_id >= num_eliminate_blocks) { + continue; + } + + for (int j = 1; j < cells.size(); ++j) { + int camera_block_id = cells[j].block_id - num_eliminate_blocks; + DCHECK_GE(camera_block_id, 0); + DCHECK_LT(camera_block_id, visibility->size()); + (*visibility)[camera_block_id].insert(block_id); + } + } +} + +Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility) { + const time_t start_time = time(NULL); + // Compute the number of e_blocks/point blocks. Since the visibility + // set for each e_block/camera contains the set of e_blocks/points + // visible to it, we find the maximum across all visibility sets. + int num_points = 0; + for (int i = 0; i < visibility.size(); i++) { + if (visibility[i].size() > 0) { + num_points = max(num_points, (*visibility[i].rbegin()) + 1); + } + } + + // Invert the visibility. The input is a camera->point mapping, + // which tells us which points are visible in which + // cameras. However, to compute the sparsity structure of the Schur + // Complement efficiently, its better to have the point->camera + // mapping. + vector<set<int> > inverse_visibility(num_points); + for (int i = 0; i < visibility.size(); i++) { + const set<int>& visibility_set = visibility[i]; + for (set<int>::const_iterator it = visibility_set.begin(); + it != visibility_set.end(); + ++it) { + inverse_visibility[*it].insert(i); + } + } + + // Map from camera pairs to number of points visible to both cameras + // in the pair. + HashMap<pair<int, int>, int > camera_pairs; + + // Count the number of points visible to each camera/f_block pair. + for (vector<set<int> >::const_iterator it = inverse_visibility.begin(); + it != inverse_visibility.end(); + ++it) { + const set<int>& inverse_visibility_set = *it; + for (set<int>::const_iterator camera1 = inverse_visibility_set.begin(); + camera1 != inverse_visibility_set.end(); + ++camera1) { + set<int>::const_iterator camera2 = camera1; + for (++camera2; camera2 != inverse_visibility_set.end(); ++camera2) { + ++(camera_pairs[make_pair(*camera1, *camera2)]); + } + } + } + + Graph<int>* graph = new Graph<int>(); + + // Add vertices and initialize the pairs for self edges so that self + // edges are guaranteed. This is needed for the Canonical views + // algorithm to work correctly. + static const double kSelfEdgeWeight = 1.0; + for (int i = 0; i < visibility.size(); ++i) { + graph->AddVertex(i); + graph->AddEdge(i, i, kSelfEdgeWeight); + } + + // Add an edge for each camera pair. + for (HashMap<pair<int, int>, int>::const_iterator it = camera_pairs.begin(); + it != camera_pairs.end(); + ++it) { + const int camera1 = it->first.first; + const int camera2 = it->first.second; + CHECK_NE(camera1, camera2); + + const int count = it->second; + // Static cast necessary for Windows. + const double weight = static_cast<double>(count) / + (sqrt(static_cast<double>(visibility[camera1].size() * visibility[camera2].size()))); + graph->AddEdge(camera1, camera2, weight); + } + + VLOG(2) << "Schur complement graph time: " << (time(NULL) - start_time); + return graph; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility.h b/extern/libmv/third_party/ceres/internal/ceres/visibility.h new file mode 100644 index 00000000000..692dd87201e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility.h @@ -0,0 +1,77 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: kushalav@google.com (Avanish Kushal) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Functions to manipulate visibility information from the block +// structure of sparse matrices. + +#ifndef CERES_INTERNAL_VISIBILITY_H_ +#define CERES_INTERNAL_VISIBILITY_H_ + +#include <set> +#include <vector> +#include "ceres/graph.h" + +namespace ceres { +namespace internal { + +class CompressedRowBlockStructure; + +// Given a compressed row block structure, computes the set of +// e_blocks "visible" to each f_block. If an e_block co-occurs with an +// f_block in a residual block, it is visible to the f_block. The +// first num_eliminate_blocks columns blocks are e_blocks and the rest +// f_blocks. +// +// In a structure from motion problem, e_blocks correspond to 3D +// points and f_blocks correspond to cameras. +void ComputeVisibility(const CompressedRowBlockStructure& block_structure, + int num_eliminate_blocks, + vector<set<int> >* visibility); + +// Given f_block visibility as computed by the ComputeVisibility +// function above, construct and return a graph whose vertices are +// f_blocks and an edge connects two vertices if they have atleast one +// e_block in common. The weight of this edge is normalized dot +// product between the visibility vectors of the two +// vertices/f_blocks. +// +// This graph reflects the sparsity structure of reduced camera +// matrix/Schur complement matrix obtained by eliminating the e_blocks +// from the normal equations. +// +// Caller acquires ownership of the returned Graph pointer +// (heap-allocated). +Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_VISIBILITY_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc new file mode 100644 index 00000000000..aca77528215 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc @@ -0,0 +1,611 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/visibility_based_preconditioner.h" + +#include <algorithm> +#include <functional> +#include <iterator> +#include <numeric> +#include <set> +#include <utility> +#include <vector> +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_random_access_sparse_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/canonical_views_clustering.h" +#include "ceres/collections_port.h" +#include "ceres/detect_structure.h" +#include "ceres/graph.h" +#include "ceres/graph_algorithms.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/visibility.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// TODO(sameeragarwal): Currently these are magic weights for the +// preconditioner construction. Move these higher up into the Options +// struct and provide some guidelines for choosing them. +// +// This will require some more work on the clustering algorithm and +// possibly some more refactoring of the code. +static const double kSizePenaltyWeight = 3.0; +static const double kSimilarityPenaltyWeight = 0.0; + +#ifndef CERES_NO_SUITESPARSE +VisibilityBasedPreconditioner::VisibilityBasedPreconditioner( + const CompressedRowBlockStructure& bs, + const LinearSolver::Options& options) + : options_(options), + num_blocks_(0), + num_clusters_(0), + factor_(NULL) { + CHECK_GT(options_.num_eliminate_blocks, 0); + CHECK(options_.preconditioner_type == SCHUR_JACOBI || + options_.preconditioner_type == CLUSTER_JACOBI || + options_.preconditioner_type == CLUSTER_TRIDIAGONAL) + << "Unknown preconditioner type: " << options_.preconditioner_type; + num_blocks_ = bs.cols.size() - options_.num_eliminate_blocks; + CHECK_GT(num_blocks_, 0) + << "Jacobian should have atleast 1 f_block for " + << "visibility based preconditioning."; + + // Vector of camera block sizes + block_size_.resize(num_blocks_); + for (int i = 0; i < num_blocks_; ++i) { + block_size_[i] = bs.cols[i + options_.num_eliminate_blocks].size; + } + + const time_t start_time = time(NULL); + switch (options_.preconditioner_type) { + case SCHUR_JACOBI: + ComputeSchurJacobiSparsity(bs); + break; + case CLUSTER_JACOBI: + ComputeClusterJacobiSparsity(bs); + break; + case CLUSTER_TRIDIAGONAL: + ComputeClusterTridiagonalSparsity(bs); + break; + default: + LOG(FATAL) << "Unknown preconditioner type"; + } + const time_t structure_time = time(NULL); + InitStorage(bs); + const time_t storage_time = time(NULL); + InitEliminator(bs); + const time_t eliminator_time = time(NULL); + + // Allocate temporary storage for a vector used during + // RightMultiply. + tmp_rhs_ = CHECK_NOTNULL(ss_.CreateDenseVector(NULL, + m_->num_rows(), + m_->num_rows())); + const time_t init_time = time(NULL); + VLOG(2) << "init time: " + << init_time - start_time + << " structure time: " << structure_time - start_time + << " storage time:" << storage_time - structure_time + << " eliminator time: " << eliminator_time - storage_time; +} + +VisibilityBasedPreconditioner::~VisibilityBasedPreconditioner() { + if (factor_ != NULL) { + ss_.Free(factor_); + factor_ = NULL; + } + if (tmp_rhs_ != NULL) { + ss_.Free(tmp_rhs_); + tmp_rhs_ = NULL; + } +} + +// Determine the sparsity structure of the SCHUR_JACOBI +// preconditioner. SCHUR_JACOBI is an extreme case of a visibility +// based preconditioner where each camera block corresponds to a +// cluster and there is no interaction between clusters. +void VisibilityBasedPreconditioner::ComputeSchurJacobiSparsity( + const CompressedRowBlockStructure& bs) { + num_clusters_ = num_blocks_; + cluster_membership_.resize(num_blocks_); + cluster_pairs_.clear(); + + // Each camea block is a member of its own cluster and the only + // cluster pairs are the self edges (i,i). + for (int i = 0; i < num_clusters_; ++i) { + cluster_membership_[i] = i; + cluster_pairs_.insert(make_pair(i, i)); + } +} + +// Determine the sparsity structure of the CLUSTER_JACOBI +// preconditioner. It clusters cameras using their scene +// visibility. The clusters form the diagonal blocks of the +// preconditioner matrix. +void VisibilityBasedPreconditioner::ComputeClusterJacobiSparsity( + const CompressedRowBlockStructure& bs) { + vector<set<int> > visibility; + ComputeVisibility(bs, options_.num_eliminate_blocks, &visibility); + CHECK_EQ(num_blocks_, visibility.size()); + ClusterCameras(visibility); + cluster_pairs_.clear(); + for (int i = 0; i < num_clusters_; ++i) { + cluster_pairs_.insert(make_pair(i, i)); + } +} + +// Determine the sparsity structure of the CLUSTER_TRIDIAGONAL +// preconditioner. It clusters cameras using using the scene +// visibility and then finds the strongly interacting pairs of +// clusters by constructing another graph with the clusters as +// vertices and approximating it with a degree-2 maximum spanning +// forest. The set of edges in this forest are the cluster pairs. +void VisibilityBasedPreconditioner::ComputeClusterTridiagonalSparsity( + const CompressedRowBlockStructure& bs) { + vector<set<int> > visibility; + ComputeVisibility(bs, options_.num_eliminate_blocks, &visibility); + CHECK_EQ(num_blocks_, visibility.size()); + ClusterCameras(visibility); + + // Construct a weighted graph on the set of clusters, where the + // edges are the number of 3D points/e_blocks visible in both the + // clusters at the ends of the edge. Return an approximate degree-2 + // maximum spanning forest of this graph. + vector<set<int> > cluster_visibility; + ComputeClusterVisibility(visibility, &cluster_visibility); + scoped_ptr<Graph<int> > cluster_graph( + CHECK_NOTNULL(CreateClusterGraph(cluster_visibility))); + scoped_ptr<Graph<int> > forest( + CHECK_NOTNULL(Degree2MaximumSpanningForest(*cluster_graph))); + ForestToClusterPairs(*forest, &cluster_pairs_); +} + +// Allocate storage for the preconditioner matrix. +void VisibilityBasedPreconditioner::InitStorage( + const CompressedRowBlockStructure& bs) { + ComputeBlockPairsInPreconditioner(bs); + m_.reset(new BlockRandomAccessSparseMatrix(block_size_, block_pairs_)); +} + +// Call the canonical views algorithm and cluster the cameras based on +// their visibility sets. The visibility set of a camera is the set of +// e_blocks/3D points in the scene that are seen by it. +// +// The cluster_membership_ vector is updated to indicate cluster +// memberships for each camera block. +void VisibilityBasedPreconditioner::ClusterCameras( + const vector<set<int> >& visibility) { + scoped_ptr<Graph<int> > schur_complement_graph( + CHECK_NOTNULL(CreateSchurComplementGraph(visibility))); + + CanonicalViewsClusteringOptions options; + options.size_penalty_weight = kSizePenaltyWeight; + options.similarity_penalty_weight = kSimilarityPenaltyWeight; + + vector<int> centers; + HashMap<int, int> membership; + ComputeCanonicalViewsClustering(*schur_complement_graph, + options, + ¢ers, + &membership); + num_clusters_ = centers.size(); + CHECK_GT(num_clusters_, 0); + VLOG(2) << "num_clusters: " << num_clusters_; + FlattenMembershipMap(membership, &cluster_membership_); +} + +// Compute the block sparsity structure of the Schur complement +// matrix. For each pair of cameras contributing a non-zero cell to +// the schur complement, determine if that cell is present in the +// preconditioner or not. +// +// A pair of cameras contribute a cell to the preconditioner if they +// are part of the same cluster or if the the two clusters that they +// belong have an edge connecting them in the degree-2 maximum +// spanning forest. +// +// For example, a camera pair (i,j) where i belonges to cluster1 and +// j belongs to cluster2 (assume that cluster1 < cluster2). +// +// The cell corresponding to (i,j) is present in the preconditioner +// if cluster1 == cluster2 or the pair (cluster1, cluster2) were +// connected by an edge in the degree-2 maximum spanning forest. +// +// Since we have already expanded the forest into a set of camera +// pairs/edges, including self edges, the check can be reduced to +// checking membership of (cluster1, cluster2) in cluster_pairs_. +void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner( + const CompressedRowBlockStructure& bs) { + block_pairs_.clear(); + for (int i = 0; i < num_blocks_; ++i) { + block_pairs_.insert(make_pair(i, i)); + } + + int r = 0; + set<pair<int, int> > skipped_pairs; + const int num_row_blocks = bs.rows.size(); + const int num_eliminate_blocks = options_.num_eliminate_blocks; + + // Iterate over each row of the matrix. The block structure of the + // matrix is assumed to be sorted in order of the e_blocks/point + // blocks. Thus all row blocks containing an e_block/point occur + // contiguously. Further, if present, an e_block is always the first + // parameter block in each row block. These structural assumptions + // are common to all Schur complement based solvers in Ceres. + // + // For each e_block/point block we identify the set of cameras + // seeing it. The cross product of this set with itself is the set + // of non-zero cells contibuted by this e_block. + // + // The time complexity of this is O(nm^2) where, n is the number of + // 3d points and m is the maximum number of cameras seeing any + // point, which for most scenes is a fairly small number. + while (r < num_row_blocks) { + int e_block_id = bs.rows[r].cells.front().block_id; + if (e_block_id >= num_eliminate_blocks) { + // Skip the rows whose first block is an f_block. + break; + } + + set<int> f_blocks; + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + if (row.cells.front().block_id != e_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first block + // since it is the one to be eliminated and adding the rest to + // the list of f_blocks associated with this e_block. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + const int f_block_id = cell.block_id - num_eliminate_blocks; + CHECK_GE(f_block_id, 0); + f_blocks.insert(f_block_id); + } + } + + for (set<int>::const_iterator block1 = f_blocks.begin(); + block1 != f_blocks.end(); + ++block1) { + set<int>::const_iterator block2 = block1; + ++block2; + for (; block2 != f_blocks.end(); ++block2) { + if (IsBlockPairInPreconditioner(*block1, *block2)) { + block_pairs_.insert(make_pair(*block1, *block2)); + } else { + skipped_pairs.insert(make_pair(*block1, *block2)); + } + } + } + } + + // The remaining rows which do not contain any e_blocks. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); + for (int i = 0; i < row.cells.size(); ++i) { + const int block1 = row.cells[i].block_id - num_eliminate_blocks; + for (int j = 0; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks; + if (block1 <= block2) { + if (IsBlockPairInPreconditioner(block1, block2)) { + block_pairs_.insert(make_pair(block1, block2)); + } else { + skipped_pairs.insert(make_pair(block1, block2)); + } + } + } + } + } + + VLOG(1) << "Block pair stats: " + << block_pairs_.size() << " included " + << skipped_pairs.size() << " excluded"; +} + +// Initialize the SchurEliminator. +void VisibilityBasedPreconditioner::InitEliminator( + const CompressedRowBlockStructure& bs) { + LinearSolver::Options eliminator_options; + eliminator_options.num_eliminate_blocks = options_.num_eliminate_blocks; + eliminator_options.num_threads = options_.num_threads; + eliminator_options.constant_sparsity = true; + + DetectStructure(bs, options_.num_eliminate_blocks, + &eliminator_options.row_block_size, + &eliminator_options.e_block_size, + &eliminator_options.f_block_size); + + eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); + eliminator_->Init(options_.num_eliminate_blocks, &bs); +} + +// Compute the values of the preconditioner matrix and factorize it. +bool VisibilityBasedPreconditioner::Compute(const BlockSparseMatrixBase& A, + const double* D) { + const time_t start_time = time(NULL); + const int num_rows = m_->num_rows(); + CHECK_GT(num_rows, 0); + + // We need a dummy rhs vector and a dummy b vector since the Schur + // eliminator combines the computation of the reduced camera matrix + // with the computation of the right hand side of that linear + // system. + // + // TODO(sameeragarwal): Perhaps its worth refactoring the + // SchurEliminator::Eliminate function to allow NULL for the rhs. As + // of now it does not seem to be worth the effort. + Vector rhs = Vector::Zero(m_->num_rows()); + Vector b = Vector::Zero(A.num_rows()); + + // Compute a subset of the entries of the Schur complement. + eliminator_->Eliminate(&A, b.data(), D, m_.get(), rhs.data()); + + // Try factorizing the matrix. For SCHUR_JACOBI and CLUSTER_JACOBI, + // this should always succeed modulo some numerical/conditioning + // problems. For CLUSTER_TRIDIAGONAL, in general the preconditioner + // matrix as constructed is not positive definite. However, we will + // go ahead and try factorizing it. If it works, great, otherwise we + // scale all the cells in the preconditioner corresponding to the + // edges in the degree-2 forest and that guarantees positive + // definiteness. The proof of this fact can be found in Lemma 1 in + // "Visibility Based Preconditioning for Bundle Adjustment". + // + // Doing the factorization like this saves us matrix mass when + // scaling is not needed, which is quite often in our experience. + bool status = Factorize(); + + // The scaling only affects the tri-diagonal case, since + // ScaleOffDiagonalBlocks only pays attenion to the cells that + // belong to the edges of the degree-2 forest. In the SCHUR_JACOBI + // and the CLUSTER_JACOBI cases, the preconditioner is guaranteed to + // be positive semidefinite. + if (!status && options_.preconditioner_type == CLUSTER_TRIDIAGONAL) { + VLOG(1) << "Unscaled factorization failed. Retrying with off-diagonal " + << "scaling"; + ScaleOffDiagonalCells(); + status = Factorize(); + } + + VLOG(2) << "Compute time: " << time(NULL) - start_time; + return status; +} + +// Consider the preconditioner matrix as meta-block matrix, whose +// blocks correspond to the clusters. Then cluster pairs corresponding +// to edges in the degree-2 forest are off diagonal entries of this +// matrix. Scaling these off-diagonal entries by 1/2 forces this +// matrix to be positive definite. +void VisibilityBasedPreconditioner::ScaleOffDiagonalCells() { + for (set< pair<int, int> >::const_iterator it = block_pairs_.begin(); + it != block_pairs_.end(); + ++it) { + const int block1 = it->first; + const int block2 = it->second; + if (!IsBlockPairOffDiagonal(block1, block2)) { + continue; + } + + int r, c, row_stride, col_stride; + CellInfo* cell_info = m_->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + CHECK(cell_info != NULL) + << "Cell missing for block pair (" << block1 << "," << block2 << ")" + << " cluster pair (" << cluster_membership_[block1] + << " " << cluster_membership_[block2] << ")"; + + // Ah the magic of tri-diagonal matrices and diagonal + // dominance. See Lemma 1 in "Visibility Based Preconditioning + // For Bundle Adjustment". + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size_[block1], block_size_[block2]) *= 0.5; + } +} + +// Compute the sparse Cholesky factorization of the preconditioner +// matrix. +bool VisibilityBasedPreconditioner::Factorize() { + // Extract the TripletSparseMatrix that is used for actually storing + // S and convert it into a cholmod_sparse object. + cholmod_sparse* lhs = ss_.CreateSparseMatrix( + down_cast<BlockRandomAccessSparseMatrix*>( + m_.get())->mutable_matrix()); + + // The matrix is symmetric, and the upper triangular part of the + // matrix contains the values. + lhs->stype = 1; + + // Symbolic factorization is computed if we don't already have one + // handy. + if (factor_ == NULL) { + factor_ = ss_.AnalyzeCholesky(lhs); + } + + bool status = ss_.Cholesky(lhs, factor_); + ss_.Free(lhs); + return status; +} + +void VisibilityBasedPreconditioner::RightMultiply(const double* x, + double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + SuiteSparse* ss = const_cast<SuiteSparse*>(&ss_); + + const int num_rows = m_->num_rows(); + memcpy(CHECK_NOTNULL(tmp_rhs_)->x, x, m_->num_rows() * sizeof(*x)); + cholmod_dense* solution = CHECK_NOTNULL(ss->Solve(factor_, tmp_rhs_)); + memcpy(y, solution->x, sizeof(*y) * num_rows); + ss->Free(solution); +} + +int VisibilityBasedPreconditioner::num_rows() const { + return m_->num_rows(); +} + +// Classify camera/f_block pairs as in and out of the preconditioner, +// based on whether the cluster pair that they belong to is in the +// preconditioner or not. +bool VisibilityBasedPreconditioner::IsBlockPairInPreconditioner( + const int block1, + const int block2) const { + int cluster1 = cluster_membership_[block1]; + int cluster2 = cluster_membership_[block2]; + if (cluster1 > cluster2) { + std::swap(cluster1, cluster2); + } + return (cluster_pairs_.count(make_pair(cluster1, cluster2)) > 0); +} + +bool VisibilityBasedPreconditioner::IsBlockPairOffDiagonal( + const int block1, + const int block2) const { + return (cluster_membership_[block1] != cluster_membership_[block2]); +} + +// Convert a graph into a list of edges that includes self edges for +// each vertex. +void VisibilityBasedPreconditioner::ForestToClusterPairs( + const Graph<int>& forest, + HashSet<pair<int, int> >* cluster_pairs) const { + CHECK_NOTNULL(cluster_pairs)->clear(); + const HashSet<int>& vertices = forest.vertices(); + CHECK_EQ(vertices.size(), num_clusters_); + + // Add all the cluster pairs corresponding to the edges in the + // forest. + for (HashSet<int>::const_iterator it1 = vertices.begin(); + it1 != vertices.end(); + ++it1) { + const int cluster1 = *it1; + cluster_pairs->insert(make_pair(cluster1, cluster1)); + const HashSet<int>& neighbors = forest.Neighbors(cluster1); + for (HashSet<int>::const_iterator it2 = neighbors.begin(); + it2 != neighbors.end(); + ++it2) { + const int cluster2 = *it2; + if (cluster1 < cluster2) { + cluster_pairs->insert(make_pair(cluster1, cluster2)); + } + } + } +} + +// The visibilty set of a cluster is the union of the visibilty sets +// of all its cameras. In other words, the set of points visible to +// any camera in the cluster. +void VisibilityBasedPreconditioner::ComputeClusterVisibility( + const vector<set<int> >& visibility, + vector<set<int> >* cluster_visibility) const { + CHECK_NOTNULL(cluster_visibility)->resize(0); + cluster_visibility->resize(num_clusters_); + for (int i = 0; i < num_blocks_; ++i) { + const int cluster_id = cluster_membership_[i]; + (*cluster_visibility)[cluster_id].insert(visibility[i].begin(), + visibility[i].end()); + } +} + +// Construct a graph whose vertices are the clusters, and the edge +// weights are the number of 3D points visible to cameras in both the +// vertices. +Graph<int>* VisibilityBasedPreconditioner::CreateClusterGraph( + const vector<set<int> >& cluster_visibility) const { + Graph<int>* cluster_graph = new Graph<int>; + + for (int i = 0; i < num_clusters_; ++i) { + cluster_graph->AddVertex(i); + } + + for (int i = 0; i < num_clusters_; ++i) { + const set<int>& cluster_i = cluster_visibility[i]; + for (int j = i+1; j < num_clusters_; ++j) { + vector<int> intersection; + const set<int>& cluster_j = cluster_visibility[j]; + set_intersection(cluster_i.begin(), cluster_i.end(), + cluster_j.begin(), cluster_j.end(), + back_inserter(intersection)); + + if (intersection.size() > 0) { + // Clusters interact strongly when they share a large number + // of 3D points. The degree-2 maximum spanning forest + // alorithm, iterates on the edges in decreasing order of + // their weight, which is the number of points shared by the + // two cameras that it connects. + cluster_graph->AddEdge(i, j, intersection.size()); + } + } + } + return cluster_graph; +} + +// Canonical views clustering returns a HashMap from vertices to +// cluster ids. Convert this into a flat array for quick lookup. It is +// possible that some of the vertices may not be associated with any +// cluster. In that case, randomly assign them to one of the clusters. +void VisibilityBasedPreconditioner::FlattenMembershipMap( + const HashMap<int, int>& membership_map, + vector<int>* membership_vector) const { + CHECK_NOTNULL(membership_vector)->resize(0); + membership_vector->resize(num_blocks_, -1); + // Iterate over the cluster membership map and update the + // cluster_membership_ vector assigning arbitrary cluster ids to + // the few cameras that have not been clustered. + for (HashMap<int, int>::const_iterator it = membership_map.begin(); + it != membership_map.end(); + ++it) { + const int camera_id = it->first; + int cluster_id = it->second; + + // If the view was not clustered, randomly assign it to one of the + // clusters. This preserves the mathematical correctness of the + // preconditioner. If there are too many views which are not + // clustered, it may lead to some quality degradation though. + // + // TODO(sameeragarwal): Check if a large number of views have not + // been clustered and deal with it? + if (cluster_id == -1) { + cluster_id = camera_id % num_clusters_; + } + + membership_vector->at(camera_id) = cluster_id; + } +} + +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h new file mode 100644 index 00000000000..fa095ca1dd8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h @@ -0,0 +1,273 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Preconditioners for linear systems that arise in Structure from +// Motion problems. VisibilityBasedPreconditioner implements three +// preconditioners: +// +// SCHUR_JACOBI +// CLUSTER_JACOBI +// CLUSTER_TRIDIAGONAL +// +// Detailed descriptions of these preconditions beyond what is +// documented here can be found in +// +// Bundle Adjustment in the Large +// S. Agarwal, N. Snavely, S. Seitz & R. Szeliski, ECCV 2010 +// http://www.cs.washington.edu/homes/sagarwal/bal.pdf +// +// Visibility Based Preconditioning for Bundle Adjustment +// A. Kushal & S. Agarwal, submitted to CVPR 2012 +// http://www.cs.washington.edu/homes/sagarwal/vbp.pdf +// +// The three preconditioners share enough code that its most efficient +// to implement them as part of the same code base. + +#ifndef CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ +#define CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/collections_port.h" +#include "ceres/graph.h" +#include "ceres/linear_solver.h" +#include "ceres/linear_operator.h" +#include "ceres/suitesparse.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +class BlockRandomAccessSparseMatrix; +class BlockSparseMatrixBase; +class CompressedRowBlockStructure; +class SchurEliminatorBase; + +// This class implements three preconditioners for Structure from +// Motion/Bundle Adjustment problems. The name +// VisibilityBasedPreconditioner comes from the fact that the sparsity +// structure of the preconditioner matrix is determined by analyzing +// the visibility structure of the scene, i.e. which cameras see which +// points. +// +// Strictly speaking, SCHUR_JACOBI is not a visibility based +// preconditioner but it is an extreme case of CLUSTER_JACOBI, where +// every cluster contains exactly one camera block. Treating it as a +// special case of CLUSTER_JACOBI makes it easy to implement as part +// of the same code base with no significant loss of performance. +// +// In the following, we will only discuss CLUSTER_JACOBI and +// CLUSTER_TRIDIAGONAL. +// +// The key idea of visibility based preconditioning is to identify +// cameras that we expect have strong interactions, and then using the +// entries in the Schur complement matrix corresponding to these +// camera pairs as an approximation to the full Schur complement. +// +// CLUSTER_JACOBI identifies these camera pairs by clustering cameras, +// and considering all non-zero camera pairs within each cluster. The +// clustering in the current implementation is done using the +// Canonical Views algorithm of Simon et al. (see +// canonical_views_clustering.h). For the purposes of clustering, the +// similarity or the degree of interaction between a pair of cameras +// is measured by counting the number of points visible in both the +// cameras. Thus the name VisibilityBasedPreconditioner. Further, if we +// were to permute the parameter blocks such that all the cameras in +// the same cluster occur contiguously, the preconditioner matrix will +// be a block diagonal matrix with blocks corresponding to the +// clusters. Thus in analogy with the Jacobi preconditioner we refer +// to this as the CLUSTER_JACOBI preconditioner. +// +// CLUSTER_TRIDIAGONAL adds more mass to the CLUSTER_JACOBI +// preconditioner by considering the interaction between clusters and +// identifying strong interactions between cluster pairs. This is done +// by constructing a weighted graph on the clusters, with the weight +// on the edges connecting two clusters proportional to the number of +// 3D points visible to cameras in both the clusters. A degree-2 +// maximum spanning forest is identified in this graph and the camera +// pairs contained in the edges of this forest are added to the +// preconditioner. The detailed reasoning for this construction is +// explained in the paper mentioned above. +// +// Degree-2 spanning trees and forests have the property that they +// correspond to tri-diagonal matrices. Thus there exist a permutation +// of the camera blocks under which the CLUSTER_TRIDIAGONAL +// preconditioner matrix is a block tridiagonal matrix, and thus the +// name for the preconditioner. +// +// Thread Safety: This class is NOT thread safe. +// +// Example usage: +// +// LinearSolver::Options options; +// options.preconditioner_type = CLUSTER_JACOBI; +// options.num_eliminate_blocks = num_points; +// VisibilityBasedPreconditioner preconditioner( +// *A.block_structure(), options); +// preconditioner.Compute(A, NULL); +// preconditioner.RightMultiply(x, y); +// + +#ifndef CERES_NO_SUITESPARSE +class VisibilityBasedPreconditioner : public LinearOperator { + public: + // Initialize the symbolic structure of the preconditioner. bs is + // the block structure of the linear system to be solved. It is used + // to determine the sparsity structure of the preconditioner matrix. + // + // It has the same structural requirement as other Schur complement + // based solvers. Please see schur_eliminator.h for more details. + // + // LinearSolver::Options::num_eliminate_blocks should be set to the + // number of e_blocks in the block structure. + // + // TODO(sameeragarwal): The use of LinearSolver::Options should + // ultimately be replaced with Preconditioner::Options and some sort + // of preconditioner factory along the lines of + // LinearSolver::CreateLinearSolver. I will wait to do this till I + // create a general purpose block Jacobi preconditioner for general + // sparse problems along with a CGLS solver. + VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, + const LinearSolver::Options& options); + virtual ~VisibilityBasedPreconditioner(); + + // Compute the numerical value of the preconditioner for the linear + // system: + // + // | A | x = |b| + // |diag(D)| |0| + // + // for some vector b. It is important that the matrix A have the + // same block structure as the one used to construct this object. + // + // D can be NULL, in which case its interpreted as a diagonal matrix + // of size zero. + bool Compute(const BlockSparseMatrixBase& A, + const double* D); + + // LinearOperator interface. Since the operator is symmetric, + // LeftMultiply and num_cols are just calls to RightMultiply and + // num_rows respectively. Compute() must be called before + // RightMultiply can be called. + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + virtual int num_rows() const; + virtual int num_cols() const { return num_rows(); } + + friend class VisibilityBasedPreconditionerTest; + private: + void ComputeSchurJacobiSparsity(const CompressedRowBlockStructure& bs); + void ComputeClusterJacobiSparsity(const CompressedRowBlockStructure& bs); + void ComputeClusterTridiagonalSparsity(const CompressedRowBlockStructure& bs); + void InitStorage(const CompressedRowBlockStructure& bs); + void InitEliminator(const CompressedRowBlockStructure& bs); + bool Factorize(); + void ScaleOffDiagonalCells(); + + void ClusterCameras(const vector< set<int> >& visibility); + void FlattenMembershipMap(const HashMap<int, int>& membership_map, + vector<int>* membership_vector) const; + void ComputeClusterVisibility(const vector<set<int> >& visibility, + vector<set<int> >* cluster_visibility) const; + Graph<int>* CreateClusterGraph(const vector<set<int> >& visibility) const; + void ForestToClusterPairs(const Graph<int>& forest, + HashSet<pair<int, int> >* cluster_pairs) const; + void ComputeBlockPairsInPreconditioner(const CompressedRowBlockStructure& bs); + bool IsBlockPairInPreconditioner(int block1, int block2) const; + bool IsBlockPairOffDiagonal(int block1, int block2) const; + + LinearSolver::Options options_; + + // Number of parameter blocks in the schur complement. + int num_blocks_; + int num_clusters_; + + // Sizes of the blocks in the schur complement. + vector<int> block_size_; + + // Mapping from cameras to clusters. + vector<int> cluster_membership_; + + // Non-zero camera pairs from the schur complement matrix that are + // present in the preconditioner, sorted by row (first element of + // each pair), then column (second). + set<pair<int, int> > block_pairs_; + + // Set of cluster pairs (including self pairs (i,i)) in the + // preconditioner. + HashSet<pair<int, int> > cluster_pairs_; + scoped_ptr<SchurEliminatorBase> eliminator_; + + // Preconditioner matrix. + scoped_ptr<BlockRandomAccessSparseMatrix> m_; + + // RightMultiply is a const method for LinearOperators. It is + // implemented using CHOLMOD's sparse triangular matrix solve + // function. This however requires non-const access to the + // SuiteSparse context object, even though it does not result in any + // of the state of the preconditioner being modified. + SuiteSparse ss_; + + // Symbolic and numeric factorization of the preconditioner. + cholmod_factor* factor_; + + // Temporary vector used by RightMultiply. + cholmod_dense* tmp_rhs_; + DISALLOW_COPY_AND_ASSIGN(VisibilityBasedPreconditioner); +}; +#else // SuiteSparse +// If SuiteSparse is not compiled in, the preconditioner is not +// available. +class VisibilityBasedPreconditioner : public LinearOperator { + public: + VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, + const LinearSolver::Options& options) { + LOG(FATAL) << "Visibility based preconditioning is not available. Please " + "build Ceres with SuiteSparse."; + } + virtual ~VisibilityBasedPreconditioner() {} + virtual void RightMultiply(const double* x, double* y) const {} + virtual void LeftMultiply(const double* x, double* y) const {} + virtual int num_rows() const { return -1; } + virtual int num_cols() const { return -1; } + bool Compute(const BlockSparseMatrixBase& A, const double* D) { + return false; + } +}; +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ diff --git a/extern/libmv/third_party/ceres/mkfiles.sh b/extern/libmv/third_party/ceres/mkfiles.sh new file mode 100644 index 00000000000..d335829aa2c --- /dev/null +++ b/extern/libmv/third_party/ceres/mkfiles.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +find ./include/ -type f | sed -r 's/^\.\///' | sort > files.txt +find ./internal/ -type f | sed -r 's/^\.\///' | sort >> files.txt diff --git a/extern/libmv/third_party/ceres/patches/msvc_isfinite.patch b/extern/libmv/third_party/ceres/patches/msvc_isfinite.patch new file mode 100644 index 00000000000..c3129d8e02b --- /dev/null +++ b/extern/libmv/third_party/ceres/patches/msvc_isfinite.patch @@ -0,0 +1,15 @@ +diff --git a/internal/ceres/residual_block_utils.cc b/internal/ceres/residual_block_utils.cc +index ed3499b..28e0313 100644 +--- a/internal/ceres/residual_block_utils.cc ++++ b/internal/ceres/residual_block_utils.cc +@@ -40,6 +40,10 @@ + #include "ceres/internal/eigen.h" + #include "ceres/internal/port.h" + ++#ifdef _MSC_VER ++# define isfinite _finite ++#endif ++ + namespace ceres { + namespace internal { + diff --git a/extern/libmv/third_party/ceres/patches/series b/extern/libmv/third_party/ceres/patches/series new file mode 100644 index 00000000000..dbe955ae61e --- /dev/null +++ b/extern/libmv/third_party/ceres/patches/series @@ -0,0 +1 @@ +msvc_isfinite.patch |