diff options
| author | Sergey Sharybin <sergey.vfx@gmail.com> | 2012-09-16 16:24:37 +0400 |
|---|---|---|
| committer | Sergey Sharybin <sergey.vfx@gmail.com> | 2012-09-16 16:24:37 +0400 |
| commit | a737bc49af5a00f0a938a7721b579f9f4d271756 (patch) | |
| tree | 4119b9492363fbbce03e8485430407c8652faad8 /extern | |
| parent | d066ce9533995e8d1c712d47dfe093533d921a6f (diff) | |
Ceres: upgrade to version 1.3.0
This should contain real fixes for Windows, making it more robost and hopefully
faster (due to proper collection port) on that platform.
Also hack to fix Eigen alignment shouldn't be needed anymore.
Also on platforms which have got broken TR1 collections it's better to define
CERES_NO_TR1 instead of using Boost hacks. Made changes to Scons and CMake,
but can not check if this indeed works since i don't have OSX here.
Diffstat (limited to 'extern')
120 files changed, 5699 insertions, 1704 deletions
diff --git a/extern/libmv/libmv/tracking/track_region.cc b/extern/libmv/libmv/tracking/track_region.cc index f52919b2a61..aef448eb2b6 100644 --- a/extern/libmv/libmv/tracking/track_region.cc +++ b/extern/libmv/libmv/tracking/track_region.cc @@ -41,6 +41,80 @@ #include "libmv/multiview/homography.h" #include "libmv/numeric/numeric.h" +namespace ceres { +// 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 libmv { using ceres::Jet; diff --git a/extern/libmv/third_party/ceres/CMakeLists.txt b/extern/libmv/third_party/ceres/CMakeLists.txt index e6a9e430c47..6f893090a74 100644 --- a/extern/libmv/third_party/ceres/CMakeLists.txt +++ b/extern/libmv/third_party/ceres/CMakeLists.txt @@ -38,6 +38,7 @@ set(INC_SYS ) set(SRC + internal/ceres/array_utils.cc internal/ceres/block_evaluate_preparer.cc internal/ceres/block_jacobian_writer.cc internal/ceres/block_jacobi_preconditioner.cc @@ -53,16 +54,19 @@ set(SRC internal/ceres/conditioned_cost_function.cc internal/ceres/conjugate_gradients_solver.cc internal/ceres/corrector.cc + internal/ceres/cxsparse.cc + internal/ceres/dense_normal_cholesky_solver.cc internal/ceres/dense_qr_solver.cc internal/ceres/dense_sparse_matrix.cc internal/ceres/detect_structure.cc + internal/ceres/dogleg_strategy.cc internal/ceres/evaluator.cc internal/ceres/file.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/levenberg_marquardt_strategy.cc internal/ceres/linear_least_squares_problems.cc internal/ceres/linear_operator.cc internal/ceres/linear_solver.cc @@ -70,6 +74,7 @@ set(SRC internal/ceres/loss_function.cc internal/ceres/normal_prior.cc internal/ceres/partitioned_matrix_view.cc + internal/ceres/polynomial_solver.cc internal/ceres/problem.cc internal/ceres/problem_impl.cc internal/ceres/program.cc @@ -88,6 +93,8 @@ set(SRC internal/ceres/stringprintf.cc internal/ceres/suitesparse.cc internal/ceres/triplet_sparse_matrix.cc + internal/ceres/trust_region_minimizer.cc + internal/ceres/trust_region_strategy.cc internal/ceres/types.cc internal/ceres/visibility_based_preconditioner.cc internal/ceres/visibility.cc @@ -96,6 +103,8 @@ set(SRC include/ceres/ceres.h include/ceres/conditioned_cost_function.h include/ceres/cost_function.h + include/ceres/crs_matrix.h + include/ceres/fpclassify.h include/ceres/internal/autodiff.h include/ceres/internal/eigen.h include/ceres/internal/fixed_array.h @@ -114,6 +123,7 @@ set(SRC include/ceres/sized_cost_function.h include/ceres/solver.h include/ceres/types.h + internal/ceres/array_utils.h internal/ceres/block_evaluate_preparer.h internal/ceres/block_jacobian_writer.h internal/ceres/block_jacobi_preconditioner.h @@ -131,10 +141,13 @@ set(SRC internal/ceres/compressed_row_sparse_matrix.h internal/ceres/conjugate_gradients_solver.h internal/ceres/corrector.h + internal/ceres/cxsparse.h internal/ceres/dense_jacobian_writer.h + internal/ceres/dense_normal_cholesky_solver.h internal/ceres/dense_qr_solver.h internal/ceres/dense_sparse_matrix.h internal/ceres/detect_structure.h + internal/ceres/dogleg_strategy.h internal/ceres/evaluator.h internal/ceres/file.h internal/ceres/gradient_checking_cost_function.h @@ -143,7 +156,7 @@ set(SRC 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/levenberg_marquardt_strategy.h internal/ceres/linear_least_squares_problems.h internal/ceres/linear_operator.h internal/ceres/linear_solver.h @@ -153,6 +166,7 @@ set(SRC internal/ceres/mutex.h internal/ceres/parameter_block.h internal/ceres/partitioned_matrix_view.h + internal/ceres/polynomial_solver.h internal/ceres/problem_impl.h internal/ceres/program_evaluator.h internal/ceres/program.h @@ -168,10 +182,13 @@ set(SRC internal/ceres/solver_impl.h internal/ceres/sparse_matrix.h internal/ceres/sparse_normal_cholesky_solver.h + internal/ceres/split.h internal/ceres/stl_util.h internal/ceres/stringprintf.h internal/ceres/suitesparse.h internal/ceres/triplet_sparse_matrix.h + internal/ceres/trust_region_minimizer.h + internal/ceres/trust_region_strategy.h internal/ceres/visibility_based_preconditioner.h internal/ceres/visibility.h ) @@ -217,8 +234,17 @@ add_definitions( -D"CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {" -D"CERES_HASH_NAMESPACE_END=}}" -DCERES_NO_SUITESPARSE - -DCERES_DONT_HAVE_PROTOCOL_BUFFERS + -DCERES_NO_CXSPARSE + -DCERES_NO_PROTOCOL_BUFFERS -DCERES_RESTRICT_SCHUR_SPECIALIZATION ) +if(APPLE) + if( STREQUAL "10.5") + add_definitions( + -DCERES_NO_TR1 + ) + endif() +endif() + 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 index 6e919658f13..8b84328cf98 100644 --- a/extern/libmv/third_party/ceres/ChangeLog +++ b/extern/libmv/third_party/ceres/ChangeLog @@ -1,324 +1,524 @@ -commit ca72152362ae1f4b9928c012e74b4d49d094a4ca -Merge: d297f8d 0a04199 -Author: Keir Mierle <mierle@gmail.com> -Date: Wed May 9 13:10:59 2012 -0700 +commit 552f9f85bba89f00ca307bc18fbda1dff23bd0e4 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Fri Aug 31 07:27:22 2012 -0700 - Merge branch 'master' into windows + Various minor bug fixes to the solver logic. + + 1. CostFunction returning false is handled better. + If only the cost is being evaluated, it is possible to + use the false value as an infinite value signal/outside + a region of validity. This allows a weak form of constraint + handling. Useful for example in handling infinities. + + 2. Changed the way how the slop around zero when model_cost + is larger than the current cost. Relative instead of absolute + tolerances are used. The same logic is propagated how the + corresponding clamping of the model_cost is done. + + 3. Fixed a minor indexing bug in nist.cc. + + 4. Some minor logging fixes to nist.cc to make it more + compatible with the rest of ceres. + + Together these changes, take the successful solve count from + 41/54 to 46/54 and eliminate all NUMERICAL_FAILURE problems. + + Change-Id: If94170ea4731af5b243805c0200963dd31aa94a7 -commit 0a04199ef279cc9ea97f665fed8e7fae717813c3 -Merge: fdeb577 f2571f1 -Author: Keir Mierle <mierle@gmail.com> -Date: Wed May 9 12:54:56 2012 -0700 +commit 0b776b5cc9634d3b88d623905b96006f7647ce3e +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu Aug 30 15:26:17 2012 -0700 - Merge branch 'master' of https://code.google.com/p/ceres-solver + Update docs. + + Change-Id: I69d50bcd37aed3bea2190ca614f023e83172901b -commit fdeb5772cc5eeebca4d776d220d80cc91b6d0f74 -Author: Keir Mierle <mierle@gmail.com> -Date: Wed May 9 07:38:07 2012 -0700 +commit 2d7176ad7c8fb7238ca8abd6de73415d95877494 +Author: Petter Strandmark <petter.strandmark@gmail.com> +Date: Thu Aug 30 19:51:24 2012 -0700 - Support varying numbers of residuals in autodiff. + max_consecutive_nonmonotonic_steps should be int - 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. + Found via Visual Studio warning. - 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. + Change-Id: Id2cd7de562dfc8cd35df5d5f5220dd2d7350eb2c -commit da3e0563cc12e08e7b3e0fbf11d9cc8cfe9658aa +commit 1a89bcc94e88933f89b20427a45bc40cdd23c056 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed May 9 11:57:47 2012 -0700 +Date: Thu Aug 30 15:26:17 2012 -0700 - Typo corrections in the documentation from Bing + Better reporting on the NIST problems. + + Change-Id: I7cf774ec3242c0612dbe52fc233c3fc6cff3f031 -commit aa9526d8e8fb34c23d63e3af5bf9239b0c4ea603 +commit ea11704857a1e4a735e096896e4d775d83981499 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue May 8 21:22:09 2012 -0700 +Date: Wed Aug 29 18:18:48 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. + Basic harness for testing NIST problems. + + Change-Id: I5baaa24dbf0506ceedf4a9be4ed17c84974d71a1 -commit f2571f186850ed3dd316236ac4be488979df7d30 +commit 98bf14d2b95386c2c4a6c29154637943dae4c36c Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed May 9 11:57:47 2012 -0700 +Date: Thu Aug 30 10:26:44 2012 -0700 - Typo corrections in the documentation from Bing + Miscellaneous fixes. + + Change-Id: I521e11f2d20bf24960bbc6b5dab4ec8bb1503d23 -commit 8f7f11ff7d07737435428a2620c52419cf99f98e -Merge: e6c17c4 eaccbb3 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed May 9 11:34:15 2012 -0700 +commit 1e3cbd9a4442cdd8fda43a7fb452f19dac8c74af +Author: Petter Strandmark <strandmark@google.com> +Date: Wed Aug 29 09:39:56 2012 -0700 - Merge branch 'master' of https://code.google.com/p/ceres-solver + Caching the symbolic Cholesky factorization when using CXSparse + + Average factorization times for bundle adjustment test problem: + SuiteSparse: 0.2794 s. + CXSparse: 0.4039 s. + CXSparse cached: 0.2399 s. + + CXSparse will still be slower, though, because it has to compute + the transpose and J^T * J. + + Change-Id: If9cdaa3dd520bee84b56e5fd4953b56a93db6bde -commit e6c17c4c9d9307218f6f739cea39bc2d87733d4d +commit 8b64140878ccd1e183d3715c38942a81fdecefde Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue May 8 21:22:09 2012 -0700 +Date: Wed Aug 29 05:41:22 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. + Documentation update + + Change-Id: I271a0422e7f6f42bcfd1dc6b5dc10c7a18f6a179 -commit eaccbb345614c0d24c5e21fa931f470cfda874df -Author: Keir Mierle <mierle@gmail.com> -Date: Wed May 9 05:31:29 2012 -0700 +commit a5353acd85a9fd19370b3d74035d87b0f0bac230 +Author: Petter Strandmark <petter.strandmark@gmail.com> +Date: Tue Aug 28 18:16:41 2012 -0700 - Remove unused template parameter from VariadicEvaluate. + Adding gflags include to test_util.cc + + test_util seems to need gflags. + + Change-Id: I0c4757960f8ac69ad599c138aea58e3c88a4ea28 -commit 82f4b88c34b0b2cf85064e5fc20e374e978b2e3b -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun May 6 21:05:28 2012 -0700 +commit 87ca1b2ba28ec512752bbcf5fc994ce1434eb765 +Author: Petter Strandmark <petter.strandmark@gmail.com> +Date: Tue Aug 28 18:05:20 2012 -0700 - Extend support writing linear least squares problems to disk. + Changing random.h to use cstdlib for Windows compability. - 1. Make the mechanism for writing problems to disk, generic and - controllable using an enum DumpType visible in the API. + As discussed with Sameer today. - 2. Instead of single file containing protocol buffers, now matrices can - be written in a matlab/octave friendly format. This is now the default. + Change-Id: If3d0284830c6591c71cc77b8400cafb45c0da61f + +commit aeb00a07323808a0a1816e733ad18a87d5109ea3 +Author: Petter Strandmark <strandmark@google.com> +Date: Mon Aug 27 22:22:57 2012 -0700 + + Removing gomp for Visual Studio - 3. The support for writing problems to disk is moved into - linear_least_squares_problem.cc/h + Linking currently fails in Visual Studio due to a missing library + "gomp.lib". This is not needed in Visual Studio. OpenMP works + without it. + + Change-Id: I39e204a8dd4f1b7425df7d4b222d86a8bb961432 + +commit 6f362464ba99b800494d2f15c27768a342ddaa68 +Author: Markus Moll <markus.moll@esat.kuleuven.be> +Date: Tue Aug 28 01:03:38 2012 +0200 + + Add some tests for DoglegStrategy. - 4. SparseMatrix now has a ToTextFile virtual method which is - implemented by each of its subclasses to write a (i,j,s) triplets. + Not necessarily a complete set. - 5. Minor changes to simple_bundle_adjuster to enable logging at startup. + Change-Id: I14eb3a38c6fe976c8212f3934655411b6d1e0aa4 -commit d297f8d3d3f5025c24752f0f4c1ec2469a769f99 -Merge: 7e74d81 f8bd7fa -Author: Keir Mierle <mierle@gmail.com> -Date: Tue May 8 05:39:56 2012 -0700 +commit 122cf836a6dc9726489ce2fbecc6143bddc1caaf +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Fri Aug 24 16:28:27 2012 -0700 - Merge branch 'master' into windows + Documentation update. + + Change-Id: I0a3c5ae4bc981a8f5bdd5a8905f923dc5f09a024 -commit f8bd7fa9aa9dbf64b6165606630287cf8cf21194 +commit 69081719f73da8de2935774a42d237837a91952a Author: Keir Mierle <mierle@gmail.com> -Date: Tue May 8 05:39:32 2012 -0700 +Date: Mon Aug 27 13:28:56 2012 -0700 - Small tweaks to the block jacobi preconditioner. + Remove unnecessary overload for hash<> + + The overload for pointers in hash tables was applied in normal + usage of schur_ordering.cc. However, the tests did not include the + overload since they only included collections_port.h. As a result, + the routines in schur_ordering.cc were using a different hash + function than that inside the tests. + + The fix is to remove the specialization. If this breaks one of the + compiler configurations, we will find a workaround at that time. + + Change-Id: Idbf60415d5e2aec0c865b514ad0c577d21b91405 -commit 7e74d81ad57a159f14110eb5348b3bc7990b8bd4 -Merge: ecd7c8d e2a6cdc -Author: Keir Mierle <mierle@gmail.com> -Date: Mon May 7 07:02:49 2012 -0700 +commit 1762420b6ed76b1c4d30b913b2cac1927b666534 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed Aug 22 10:01:31 2012 -0700 - Merge branch 'master' into windows + Update changelog. + + Change-Id: Idf5af69d5a9dbe35f58e30a8afcbfcd29bb7ebfe -commit e2a6cdc0816af9d0c77933f5017f137da3d52a35 +commit 976ab7aca908309b8282cb40bc080ca859136854 Author: Keir Mierle <mierle@gmail.com> -Date: Mon May 7 06:39:56 2012 -0700 +Date: Thu Aug 23 18:21:36 2012 -0700 - Address some of the comments on CGNR patch + Remove Google-era vestigial unit test. - - 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. + Change-Id: Ia7a295a5c759a17c1675a3055d287d3e40e9e0fe -commit 1b95dc580aa5d89be021c0915e26df83f18013bb -Merge: 211812a 7646039 +commit 6ad6257de0e2152ac5e77dc003758de45187d6ea Author: Keir Mierle <mierle@gmail.com> -Date: Mon May 7 04:34:10 2012 -0700 +Date: Wed Aug 22 11:10:31 2012 -0700 - Merge branch 'master' of https://code.google.com/p/ceres-solver + Add a workaround for an Android NDK compiler bug. + + On certain NDK build configurations, one of the innermost + parts of the Schur eliminator would get compiled + incorrectly. The compiler changed a -= to a +=. + + The normal Ceres unit tests caught the problem; however, + since it is not possible to build the tests with the NDK + (only with the standalone toolchain) this was difficult to + track down. Finding the issue involved pasting the schur + eliminator unit test inside of solver_impl.cc and other such + hacks. + + Change-Id: Ie91bb545d74fe39f0c8cbd1a6eb69ee4d8b25fb2 -commit 211812a57360d2011cbcfd115cd55e0eb73600db -Author: Keir Mierle <mierle@gmail.com> -Date: Mon May 7 04:33:50 2012 -0700 +commit aecb2dc92b4aa7f3bf77a1ac918e62953602392b +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed Aug 22 10:08:17 2012 -0700 - Better error handling in bundle_adjuster.cc + Fix relative path bug in bibtex call. + + Change-Id: I0d31786564320a6831259bcdf4c75a6b665c43ad -commit 7646039ad9672b267495f5b31925473ad3022ac8 +commit 1e2892009e591804df6286caebd5c960e7e3b099 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun May 6 22:02:19 2012 -0700 +Date: Tue Aug 21 18:00:54 2012 -0700 - Kashif's corrections to the docs + Update Summary::FullReport to report dogleg type. + + Change-Id: I0b4be8d7486c1c4b36b299693b3fe8b0d3426537 -commit 0d2d34148d10c5c7e924b3ca82ad2b237573ef64 +commit 295ade1122a86b83e1ea605d5ca394f315874717 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun May 6 21:16:03 2012 -0700 +Date: Wed Aug 22 06:51:22 2012 -0700 - glog minimum version requirements + Fix Eigen3 Row/Column Major storage issue. - Building Ceres requires version 0.3.1 or better of glog. - Fedora 16 ships with a busted version 0.3. + Eigen3 does not allow column vectors to be stored in row-major + format. NumericDiffCostFunction by default stores its Jacobian + matrices in row-major format. This works fine if the residual + contains more than one variable. But if the residual block + depends on one variable and has more than one residuals, the + resulting Jacobian matrix is a column matrix in row-major format + resulting in a compile time error. - issue 15 contains the gory details. + The fix is to check the template parameters and switch to column-major + storage as needed. - Added a note to the build documentation to this effect. + Thanks to Lena Gieseke for reporting this. + + Change-Id: Icc51c5b38e1f3609e0e1ecb3c4e4a02aecd72c3b -commit 39efc5ec4b64b8f5a2c5a3dbacdbc45421221547 -Author: Keir Mierle <mierle@gmail.com> -Date: Sun May 6 16:09:52 2012 -0700 +commit 9ad27e8e9fb1bbd2054e2f6ae37623e01428f1c0 +Author: Arnaud Gelas <arnaudgelas@gmail.com> +Date: Tue Aug 21 09:56:30 2012 +0200 - Fix tests broken by the CGNR change. + Add one uninstall target to remove all installed files + + Change-Id: Ifcf89a6c27b25f28403d95a50e29c093a525298f -commit 3faa08b7f7c4ac73661c6a15a6824c12080dfcb1 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun May 6 16:08:22 2012 -0700 +commit 0c3a748ee49e04fe334f8f5a433649d18003d550 +Author: Markus Moll <markus.moll@esat.kuleuven.be> +Date: Tue Aug 21 14:44:59 2012 +0200 - Formatting fixed based on Keir's comments and extended the tests + Allow equal lower and upper bound for diagonal scaling. + + This way, setting the lower and upper bound both to 1.0, one can disable + the automatic trust region scaling. + + Change-Id: Ifa317a6911b813a89c1cf7fdfde25af603705319 -commit 4f21c68409bc478c431a9b6aedf9e5cfdf11d2f3 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun May 6 15:33:47 2012 -0700 +commit 3d644b76adefac6475b91dc53c3ae5e01c4f4d66 +Author: Arnaud Gelas <arnaudgelas@gmail.com> +Date: Thu Aug 16 17:33:21 2012 +0200 - Fix the struct weak ordering used by independent set ordering, tests for it + Install headers, libraries and pdf + + Headers are installed in ${CMAKE_INSTALL_PREFIX}/include/ceres + Libraries are installed in ${CMAKE_INSTALL_PREFIX}/lib + pdf is installed in ${CMAKE_INSTALL_PREFIX}/share/ceres/docs + + Change-Id: Ic175f2c2f5fa86820a1e8c64c2ed171f4a302a68 -commit 887b156b917ccd4c172484452b059d33ea45f4f0 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun May 6 15:14:47 2012 -0700 +commit d2fb5adea4d8c2aeb43c4289c6976798a54d3cf1 +Author: Arnaud Gelas <arnaudgelas@gmail.com> +Date: Fri Aug 17 10:11:02 2012 +0200 - fix he degree ordering routine + Configure gerrit hook at CMake time + + If the source directory is a clone, at CMake time the commit-msg hook gets + downloaded and installed in the right location. + + Change-Id: I5fee17d050ca22d8b92a49fdcc2a1cd6659f209b -commit ecd7c8df2af19404dc394b36bbe96e9db3bce840 -Author: Keir Mierle <mierle@gmail.com> -Date: Sun May 6 00:09:41 2012 -0700 +commit 73166098fc4b1072adc30321c666188a3909c43c +Author: Arnaud Gelas <arnaudgelas@gmail.com> +Date: Mon Aug 20 15:40:41 2012 +0200 - First step towards windows compatibilty + Add one CMake option to build the examples. + + Currently the examples are always built. For external projects, it is useful + not to compile the examples. - 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. + Change-Id: I41d3bde19c7e742818e60f78222d39c43992ca8b -commit f7898fba1b92f0e996571b5bfa22a37f5e3644de +commit 86d4f1ba41ef14eb1b6b61a7936af83387b35eb2 Author: Keir Mierle <mierle@gmail.com> -Date: Sat May 5 20:55:08 2012 -0700 +Date: Mon Aug 20 11:52:04 2012 -0700 - Add a general sparse iterative solver: CGNR + Add missing return statement. - 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. + Change-Id: I5eaf718318e27040e3c97e32ee46cf0a11176a37 -commit 0a359d6198d257776a8831c3eb98f64ee91cf836 +commit 51eb229da34187a4e8ce73ed9cc0e731998bb2be Author: Keir Mierle <mierle@gmail.com> -Date: Sat May 5 20:33:46 2012 -0700 +Date: Mon Aug 20 11:46:12 2012 -0700 - Comment formatting. + Add Program::ToString() to aid debugging. + + Change-Id: I0ab37ed2fe0947ca87a152919d4e7dc9b56dedc6 -commit db4ec9312bb2f1ca7b2337812f6bad6cdd75b227 +commit bcc7100635e2047dc2b77df19a4ded8a6ab4d4b9 Author: Keir Mierle <mierle@gmail.com> -Date: Sat May 5 20:33:16 2012 -0700 +Date: Mon Aug 20 11:45:04 2012 -0700 - Comment formatting + Ignore minted.sty. + + Change-Id: I2467a6f801812b9007b51bf14b00757f026e4322 -commit f10163aaf3e57f52551bcd60bbdae873890a49dd +commit 9705a736dd3d6fbead0d8a6ff77102c69bbcdc08 Author: Keir Mierle <mierle@gmail.com> -Date: Fri May 4 21:33:53 2012 -0700 +Date: Mon Aug 20 11:24:05 2012 -0700 - Warn about disabled schur specializations. + Add ParameterBlock::ToString() to aid debugging. - This commit brought to you from 30,000ft. + Change-Id: Id3f5cb27b855c536dd65a986f345bd8eb2799dfa -commit ad7b2b4aaf3ccc51f2b854febd53a9df54686cfe -Author: Keir Mierle <mierle@gmail.com> -Date: Fri May 4 20:15:28 2012 -0700 +commit 0c714a70e6123ceb68e5cfcd3cfbee0d09deb1db +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Mon Aug 20 11:18:16 2012 -0700 + + Fix blanks before private in loss_function.h + + Change-Id: I068bed6431bc7c9b7958af391655df61499000b2 + +commit 51cf7cbe3bac45c6807c2703a2fc3175d76a1b47 +Author: Markus Moll <markus.moll@esat.kuleuven.be> +Date: Mon Aug 20 20:10:20 2012 +0200 + + Add the two-dimensional subspace search to DoglegStrategy + + Change-Id: I5163744c100cdf07dd93343d0734ffe0e80364f3 - Add vim swapfiles to .gitignore +commit ad1f7b772e559a911ac3a3b078b0aee1836fe785 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Mon Aug 20 11:10:34 2012 -0700 + + Add ArcTanLoss, TolerantLoss and ComposedLossFunction. + + Based on work by James Roseborough. + + Change-Id: Idc4e0b099028f67702bfc7fe3e43dbd96b6f9256 -commit 6447219826bf6e47b0c99d9ff0eaf5e2ba573d79 +commit 05292bf8fc5208b86b4a13544615b584f6efa936 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu May 3 21:53:07 2012 -0700 +Date: Mon Aug 20 07:40:45 2012 -0700 + + Add a TrustRegionStrategy::Summary object. + + Change-Id: I7caee35a3408ee4a0ec16ba407410d822929340d - 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 b12b906c4d21c3949f0dce62c4c0d083c8edecf1 +Author: Arnaud Gelas <arnaudgelas@gmail.com> +Date: Wed Aug 15 16:27:38 2012 +0200 -commit 30c5f93c7f88dec49f76168663372772e06f17f5 + Add one option to generate the PDF from CMake at build time + + Make sure pygmentize is installed + + Change-Id: I068ba45c33a8e96acc906a464b12d10d58b3e231 + +commit b9f15a59361c609ffc4a328aea9be3d265b5da81 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu May 3 10:44:43 2012 -0700 +Date: Sat Aug 18 13:06:19 2012 -0700 + + Add a dense Cholesky factorization based linear solver. + + For problems with a small number of variables, but a large + number of residuals, it is sometimes beneficial to use the + Cholesky factorization on the normal equations, instead of + the dense QR factorization of the Jacobian, even though it + is numerically the better thing to do. + + Change-Id: I3506b006195754018deec964e6e190b7e8c9ac8f - 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 b3fa009435acf476cd373052e62988f6437970b1 +Author: Arnaud Gelas <arnaudgelas@gmail.com> +Date: Fri Aug 17 10:31:41 2012 +0200 -commit f10b033eb4aca77919987bc551d16d8a88b10110 -Merge: cc38774 e0a52a9 + Set CMAKE_*_OUTPUT_DIRECTORY + + Gather + * all executables in ${CMAKE_BINARY_DIR}/bin + * all libraries (static and dynamic) in ${CMAKE_BINARY_DIR}/lib + + Change-Id: Ibc2fa1adfb6f0aea65d66d570259b79546bf3b07 + +commit 1b8a4d5d11671ed83cf6077e363dd95333f08ef8 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu May 3 08:45:20 2012 -0700 +Date: Fri Aug 17 16:49:11 2012 -0700 + + Fix a minor bug in detect_structure logging. + + Change-Id: I117f7745e4c67595b3ff9244cde82b5b5b34ee4b - Merge branch 'ctest' +commit 31c1e784ab2cb9294c6e05414cf06aae2b3766de +Author: Keir Mierle <mierle@gmail.com> +Date: Fri Aug 17 16:16:32 2012 -0700 + + Minor cleanups. + + Change-Id: Ida4866997deeaa1bc2cebd6b69313a05ac82e457 -commit e0a52a993394e73bc7f7db8d520728926feab83e +commit e83f7879a8b21c6976e116958caf35bcdcf41cb0 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu May 3 08:43:34 2012 -0700 +Date: Fri Aug 17 15:34:42 2012 -0700 - Arnaus Gelas' patch to add better path searching for gflags and glog + Fix SuiteSparse3 UFConfig.h detection really. + + Change-Id: Id187102e755b7d778dff4363f22f9a4697ed12dd -commit a9b8e815e1c026599734510399b10f4cf014c9cd +commit 96f25dc57658d296ee6b6633818b4f1e51d7d587 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu May 3 08:41:52 2012 -0700 +Date: Fri Aug 17 15:34:42 2012 -0700 + + Fix SuiteSparse3 UFConfig.h detection. + + Change-Id: Ia59aefdb0ad7f713f76ed79692f2db4fa2821e5b + +commit c497bd6cd9aa944f518aa491d3bc645851ff9594 +Author: Markus Moll <markus.moll@esat.kuleuven.be> +Date: Fri Aug 17 14:40:13 2012 +0200 - Arnaus Gelas' patch to add .gitignore + Add UFconfig and/or SuiteSparse_config test to CMakeLists.txt + + SuiteSparse 4 requires linking to libsuitesparseconfig.a. + Both SuiteSparse 3 and SuiteSparse 4 require an additional header + (either UFconfig.h or SuiteSparse_config.h) that is not found if it is + in a separate path. Therefore, add explicit checks. + + Change-Id: I699902b5db4f1b7f17134b5a54f9aa681445e294 -commit a0cefc3347c32b2065053bbaff4f34d11529d931 +commit 383c04f4236d92801c7c674892814362dedf7ad6 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu May 3 08:38:33 2012 -0700 +Date: Fri Aug 17 10:14:04 2012 -0700 - Arnaus Gelas' patch to move to Ctest + Fix QuaternionToAngleAxis to ensure rotations are between -pi and pi. + + Thanks to Guoxuan Zhang for reporting this. + + Change-Id: I2831ca3a04d5dc6467849c290461adbe23faaea3 -commit cc38774d74e287704915282425fbd16818a72ec3 -Author: Keir Mierle <mierle@gmail.com> -Date: Thu May 3 01:27:50 2012 -0700 +commit dd2b17d7dd9750801ba4720bdece2062e59b7ae3 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu Aug 16 19:34:57 2012 -0700 - Clarify ProgramEvaluator comments. + CERES_DONT_HAVE_PROTOCOL_BUFFERS -> CERES_NO_PROTOCOL_BUFFERS. + + Change-Id: I6c9f50e4c006faf4e75a8f417455db18357f3187 -commit 017c9530df557863f78212fb5ccd02814baa9fa8 +commit 8b4cb7aa2c74a0da62c638b2023566aa242af995 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed May 2 08:21:59 2012 -0700 +Date: Thu Aug 16 19:26:55 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 + Fix sparse linear algebra library logging in Summary::FullReport. + + Change-Id: Id2c902dc86c00954fde7749c7b4a67dd94215a31 -commit 92d5ab5f8ae6fe355c30b606a5f230415ee0494b -Author: Keir Mierle <mierle@gmail.com> -Date: Tue May 1 18:33:08 2012 -0700 +commit 47d26bcd3b38b5ff53b34768c33b499d47b26bd0 +Author: Markus Moll <markus.moll@esat.kuleuven.be> +Date: Thu Aug 16 00:23:38 2012 +0200 - Link BLAS explicitly on non-Mac platforms + Do not implicitly negate the step in the TrustRegionMinimizer. - Fixes issue #3. + In the TrustRegionMinimizer, the step is currently implicitly negated. + This is done so that the linearized residual is |r - J*step|^2, which + corresponds to J*step = r, so neither J nor r have to be modified. + However, it leads to the rather unintuitive situation that the strategy + returns a step in positive gradient direction, which you would expect to + increase the function value. One way is to rename the "step" parameter in + the strategy to "negative_step" and document it. + This patch instead moves the negation inside the strategy, just around + the linear solver call, so that it is done in a local context and easier + to document. + + Change-Id: Idb258149a01f61c64e22128ea221c5a30cd89c89 -commit df3e54eb4a6b001b7f0560a2da73a5bd7f18615e -Author: Keir Mierle <mierle@gmail.com> -Date: Tue May 1 18:22:51 2012 -0700 +commit 51da590c8457e6664f76fe9813425a0c71351497 +Author: Markus Moll <markus.moll@esat.kuleuven.be> +Date: Fri Aug 17 12:56:09 2012 +0200 + + Remove tmp file + + Change-Id: I07496fafae7b0c5c12cc26ae336e0db3b5592735 + +commit 7006a1f2b1701b8d89b8d1525fc0101943802221 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu Aug 16 18:04:22 2012 -0700 - Fix link order of CHOLMOD + Correct example code in Powell's function example. + + Thanks to Petter Strandmark for pointing this out. - This was working by accident due to dynamic linking. Fixes issue #2. + Change-Id: I967632235dccdb481396e94904bb911c9a1efe1e -commit f477a3835329e2b48eb20c34c631a480b0f0d5bf +commit 57a44b27bc6fc95b4e70fdc25c25c9925a2072a0 Author: Keir Mierle <mierle@gmail.com> -Date: Tue May 1 18:10:48 2012 -0700 +Date: Thu Aug 16 17:04:50 2012 -0700 - Fix Eigen search paths + Remove unnecessary flags in NDK build. - Fixes issue #1 on http://code.google.com/p/ceres-solver. + Change-Id: Ib5b4d0b7f2d898671252734978c789b8171d96a8 -commit 17fbc8ebb894c1d22bb3b0b02ea1394b580120f8 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue May 1 00:21:19 2012 -0700 +commit f21bee247251a8b2e836c215a84c4668c31d75cd +Author: Keir Mierle <mierle@gmail.com> +Date: Thu Aug 16 16:27:10 2012 -0700 - Minor changes to the documentation. Formatting, and typos. + Fix for fpclassify.h NDK porting work. + + Change-Id: I69df1b4caf2941ed96a53e35e43ec54073f84f59 -commit 8ebb0730388045570f22b89fe8672c860cd2ad1b +commit 8ceb02cb75b66602de44a35e413225386cb21c27 Author: Keir Mierle <mierle@gmail.com> -Date: Mon Apr 30 23:09:08 2012 -0700 +Date: Thu Aug 16 14:23:47 2012 -0700 - Initial commit of Ceres Solver. + Add Android NDK build files. + + This adds a Android.mk build that builds a Ceres static library + suitable for embetting in larger Android applications. This is + useful when needing to build Ceres without GPL'd components, since + the standalone toolchain (needed for the CMake Android build) does + not work with STLPort. + + Change-Id: I8d857237f6f82658741017d161b2e31d9a20e5a7 diff --git a/extern/libmv/third_party/ceres/SConscript b/extern/libmv/third_party/ceres/SConscript index c629fa00176..4567e6ffa79 100644 --- a/extern/libmv/third_party/ceres/SConscript +++ b/extern/libmv/third_party/ceres/SConscript @@ -20,9 +20,13 @@ 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') +defs.append('CERES_NO_CXSPARSE') +defs.append('CERES_NO_PROTOCOL_BUFFERS') defs.append('CERES_RESTRICT_SCHUR_SPECIALIZATION') +lif 'Mac OS X 10.5' in env['MACOSX_SDK_CHECK']: + defs.append('CERES_NO_TR1') + incs = '. ../../ ../../../Eigen3 ./include ./internal ../gflags' # work around broken hashtable in 10.5 SDK diff --git a/extern/libmv/third_party/ceres/bundle.sh b/extern/libmv/third_party/ceres/bundle.sh index 30181756c5c..8689f58ae4c 100755 --- a/extern/libmv/third_party/ceres/bundle.sh +++ b/extern/libmv/third_party/ceres/bundle.sh @@ -7,16 +7,22 @@ else exit 1 fi -repo="https://code.google.com/p/ceres-solver/" -branch="windows" +repo="https://ceres-solver.googlesource.com/ceres-solver" +branch="master" +tag="1.3.0" tmp=`mktemp -d` +checkout="$tmp/ceres" -GIT="git --git-dir $tmp/ceres/.git --work-tree $tmp/ceres" +GIT="git --git-dir $tmp/ceres/.git --work-tree $checkout" -git clone $repo $tmp/ceres +git clone $repo $checkout if [ $branch != "master" ]; then $GIT checkout -t remotes/origin/$branch +else + if [ "x$tag" != "x" ]; then + $GIT checkout $tag + fi fi $GIT log -n 50 > ChangeLog @@ -153,10 +159,19 @@ add_definitions( -D"CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {" -D"CERES_HASH_NAMESPACE_END=}}" -DCERES_NO_SUITESPARSE - -DCERES_DONT_HAVE_PROTOCOL_BUFFERS + -DCERES_NO_CXSPARSE + -DCERES_NO_PROTOCOL_BUFFERS -DCERES_RESTRICT_SCHUR_SPECIALIZATION ) +if(APPLE) + if(${CMAKE_OSX_DEPLOYMENT_TARGET} STREQUAL "10.5") + add_definitions( + -DCERES_NO_TR1 + ) + endif() +endif() + blender_add_lib(extern_ceres "\${SRC}" "\${INC}" "\${INC_SYS}") EOF @@ -183,9 +198,13 @@ 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') +defs.append('CERES_NO_CXSPARSE') +defs.append('CERES_NO_PROTOCOL_BUFFERS') defs.append('CERES_RESTRICT_SCHUR_SPECIALIZATION') +lif 'Mac OS X 10.5' in env['MACOSX_SDK_CHECK']: + defs.append('CERES_NO_TR1') + incs = '. ../../ ../../../Eigen3 ./include ./internal ../gflags' # work around broken hashtable in 10.5 SDK diff --git a/extern/libmv/third_party/ceres/files.txt b/extern/libmv/third_party/ceres/files.txt index e9d7f585260..55083572977 100644 --- a/extern/libmv/third_party/ceres/files.txt +++ b/extern/libmv/third_party/ceres/files.txt @@ -2,6 +2,8 @@ include/ceres/autodiff_cost_function.h include/ceres/ceres.h include/ceres/conditioned_cost_function.h include/ceres/cost_function.h +include/ceres/crs_matrix.h +include/ceres/fpclassify.h include/ceres/internal/autodiff.h include/ceres/internal/eigen.h include/ceres/internal/fixed_array.h @@ -20,6 +22,8 @@ include/ceres/rotation.h include/ceres/sized_cost_function.h include/ceres/solver.h include/ceres/types.h +internal/ceres/array_utils.cc +internal/ceres/array_utils.h internal/ceres/block_evaluate_preparer.cc internal/ceres/block_evaluate_preparer.h internal/ceres/block_jacobian_writer.cc @@ -52,13 +56,19 @@ internal/ceres/conjugate_gradients_solver.cc internal/ceres/conjugate_gradients_solver.h internal/ceres/corrector.cc internal/ceres/corrector.h +internal/ceres/cxsparse.cc +internal/ceres/cxsparse.h internal/ceres/dense_jacobian_writer.h +internal/ceres/dense_normal_cholesky_solver.cc +internal/ceres/dense_normal_cholesky_solver.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/dogleg_strategy.cc +internal/ceres/dogleg_strategy.h internal/ceres/evaluator.cc internal/ceres/evaluator.h internal/ceres/file.cc @@ -79,6 +89,7 @@ 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/generate_eliminator_specialization.py internal/ceres/gradient_checking_cost_function.cc internal/ceres/gradient_checking_cost_function.h internal/ceres/graph_algorithms.h @@ -88,8 +99,8 @@ 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/levenberg_marquardt_strategy.cc +internal/ceres/levenberg_marquardt_strategy.h internal/ceres/linear_least_squares_problems.cc internal/ceres/linear_least_squares_problems.h internal/ceres/linear_operator.cc @@ -106,6 +117,8 @@ internal/ceres/normal_prior.cc internal/ceres/parameter_block.h internal/ceres/partitioned_matrix_view.cc internal/ceres/partitioned_matrix_view.h +internal/ceres/polynomial_solver.cc +internal/ceres/polynomial_solver.h internal/ceres/problem.cc internal/ceres/problem_impl.cc internal/ceres/problem_impl.h @@ -136,6 +149,7 @@ 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/split.h internal/ceres/stl_util.h internal/ceres/stringprintf.cc internal/ceres/stringprintf.h @@ -143,6 +157,10 @@ internal/ceres/suitesparse.cc internal/ceres/suitesparse.h internal/ceres/triplet_sparse_matrix.cc internal/ceres/triplet_sparse_matrix.h +internal/ceres/trust_region_minimizer.cc +internal/ceres/trust_region_minimizer.h +internal/ceres/trust_region_strategy.cc +internal/ceres/trust_region_strategy.h internal/ceres/types.cc internal/ceres/visibility_based_preconditioner.cc internal/ceres/visibility_based_preconditioner.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 index e86d6993864..da9ee2c7993 100644 --- a/extern/libmv/third_party/ceres/include/ceres/autodiff_cost_function.h +++ b/extern/libmv/third_party/ceres/include/ceres/autodiff_cost_function.h @@ -163,7 +163,7 @@ class AutoDiffCostFunction : 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."; + << "number of residuals is set to ceres::DYNAMIC."; } // Takes ownership of functor. Ignores the template-provided number of @@ -174,7 +174,7 @@ class AutoDiffCostFunction : 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."; + << "number of residuals is not ceres::DYNAMIC."; SizedCostFunction<M, N0, N1, N2, N3, N4, N5>::set_num_residuals(num_residuals); } diff --git a/extern/libmv/third_party/ceres/include/ceres/cost_function.h b/extern/libmv/third_party/ceres/include/ceres/cost_function.h index 84403d90636..9b010f78f9d 100644 --- a/extern/libmv/third_party/ceres/include/ceres/cost_function.h +++ b/extern/libmv/third_party/ceres/include/ceres/cost_function.h @@ -119,7 +119,7 @@ class CostFunction { // number of outputs (residuals). vector<int16> parameter_block_sizes_; int num_residuals_; - DISALLOW_COPY_AND_ASSIGN(CostFunction); + CERES_DISALLOW_COPY_AND_ASSIGN(CostFunction); }; } // namespace ceres diff --git a/extern/libmv/third_party/ceres/include/ceres/crs_matrix.h b/extern/libmv/third_party/ceres/include/ceres/crs_matrix.h new file mode 100644 index 00000000000..c9fe8f78b7c --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/crs_matrix.h @@ -0,0 +1,65 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_CRS_MATRIX_H_ +#define CERES_PUBLIC_CRS_MATRIX_H_ + +#include <vector> +#include "ceres/internal/port.h" + +namespace ceres { + +// A compressed row sparse matrix used primarily for communicating the +// Jacobian matrix to the user. +struct CRSMatrix { + CRSMatrix() : num_rows(0), num_cols(0) {} + + int num_rows; + int num_cols; + + // A compressed row matrix stores its contents in three arrays. + // The non-zero pattern of the i^th row is given by + // + // rows[cols[i] ... cols[i + 1]] + // + // and the corresponding values by + // + // values[cols[i] ... cols[i + 1]] + // + // Thus, cols is a vector of size num_cols + 1, and rows and values + // have as many entries as number of non-zeros in the matrix. + vector<int> cols; + vector<int> rows; + vector<double> values; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_CRS_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/fpclassify.h b/extern/libmv/third_party/ceres/include/ceres/fpclassify.h new file mode 100644 index 00000000000..5a9ea1599d2 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/fpclassify.h @@ -0,0 +1,88 @@ +// 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) +// +// Portable floating point classification. The names are picked such that they +// do not collide with macros. For example, "isnan" in C99 is a macro and hence +// does not respect namespaces. +// +// TODO(keir): Finish porting! + +#ifndef CERES_PUBLIC_FPCLASSIFY_H_ +#define CERES_PUBLIC_FPCLASSIFY_H_ + +#if defined(_MSC_VER) +#include <float.h> +#endif + +namespace ceres { + +#if defined(_MSC_VER) +inline bool IsFinite (double x) { return _finite(x); } +inline bool IsInfinite(double x) { return !_finite(x) && !_isnan(x); } +inline bool IsNaN (double x) { return _isnan(x); } +inline bool IsNormal (double x) { + int classification = _fpclass(x); + return classification == _FPCLASS_NN || + classification == _FPCLASS_PN; +} +#elif defined(ANDROID) + +// On Android when using the GNU STL, the C++ fpclassify functions are not +// available. Strictly speaking, the std functions are are not standard until +// C++11. Instead use the C99 macros on Android. +inline bool IsNaN (double x) { return isnan(x); } +inline bool IsNormal (double x) { return isnormal(x); } + +// On Android NDK r6, when using STLPort, the isinf and isfinite functions are +// not available, so reimplement them. +# if defined(_STLPORT_VERSION) +inline bool IsInfinite(double x) { + return x == std::numeric_limits<double>::infinity() || + x == -std::numeric_limits<double>::infinity(); +} +inline bool IsFinite(double x) { + return !isnan(x) && !IsInfinite(x); +} +# else +inline bool IsFinite (double x) { return isfinite(x); } +inline bool IsInfinite(double x) { return isinf(x); } +# endif // defined(_STLPORT_VERSION) +#else +// These definitions are for the normal Unix suspects. +// TODO(keir): Test the "else" with more platforms. +inline bool IsFinite (double x) { return std::isfinite(x); } +inline bool IsInfinite(double x) { return std::isinf(x); } +inline bool IsNaN (double x) { return std::isnan(x); } +inline bool IsNormal (double x) { return std::isnormal(x); } +#endif + +} // namespace ceres + +#endif // CERES_PUBLIC_FPCLASSIFY_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 index 84617c4fa06..ce777d22dc7 100644 --- a/extern/libmv/third_party/ceres/include/ceres/internal/fixed_array.h +++ b/extern/libmv/third_party/ceres/include/ceres/internal/fixed_array.h @@ -34,6 +34,8 @@ #include <cstddef> #include <glog/logging.h> +#include "Eigen/Core" +#include "ceres/internal/macros.h" #include "ceres/internal/manual_constructor.h" namespace ceres { @@ -136,7 +138,6 @@ class FixedArray { // 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; }; diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/macros.h b/extern/libmv/third_party/ceres/include/ceres/internal/macros.h index 0cfd773bcca..83ec31193e7 100644 --- a/extern/libmv/third_party/ceres/include/ceres/internal/macros.h +++ b/extern/libmv/third_party/ceres/include/ceres/internal/macros.h @@ -43,11 +43,13 @@ // // 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) \ +// +// void operator=(const TypeName&); // _DISALLOW_ASSIGN + +// Note, that most uses of CERES_DISALLOW_ASSIGN and CERES_DISALLOW_COPY +// are broken semantically, one should either use disallow both or +// neither. Try to avoid these in new code. +#define CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) \ TypeName(const TypeName&); \ void operator=(const TypeName&) @@ -57,9 +59,9 @@ // 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) \ +#define CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(TypeName) \ TypeName(); \ - DISALLOW_COPY_AND_ASSIGN(TypeName) + CERES_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 @@ -151,4 +153,19 @@ char (&ArraySizeHelper(const T (&array)[N]))[N]; #define MUST_USE_RESULT #endif +// Platform independent macros to get aligned memory allocations. +// For example +// +// MyFoo my_foo CERES_ALIGN_ATTRIBUTE(16); +// +// Gives us an instance of MyFoo which is aligned at a 16 byte +// boundary. +#if defined(_MSC_VER) +#define CERES_ALIGN_ATTRIBUTE(n) __declspec(align(n)) +#define CERES_ALIGN_OF(T) __alignof(T) +#elif defined(__GNUC__) +#define CERES_ALIGN_ATTRIBUTE(n) __attribute__((aligned(n))) +#define CERES_ALIGN_OF(T) __alignof(T) +#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 index a1d1f444e36..174d35ee2bd 100644 --- a/extern/libmv/third_party/ceres/include/ceres/internal/manual_constructor.h +++ b/extern/libmv/third_party/ceres/include/ceres/internal/manual_constructor.h @@ -45,60 +45,49 @@ namespace ceres { namespace internal { -// ------- Define ALIGNED_CHAR_ARRAY -------------------------------- +// ------- Define CERES_ALIGNED_CHAR_ARRAY -------------------------------- -#ifndef ALIGNED_CHAR_ARRAY +#ifndef CERES_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) +#if !defined(CERES_ALIGN_ATTRIBUTE) +#define CERES_ALIGNED_CHAR_ARRAY you_must_define_CERES_ALIGNED_CHAR_ARRAY_for_your_compiler +#else // !defined(CERES_ALIGN_ATTRIBUTE) -#define BASE_PORT_H_ALIGNTYPE_TEMPLATE(X) \ +#define CERES_ALIGN_TYPE_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); + typedef CERES_ALIGN_ATTRIBUTE(X) char result[size]; \ + } + +CERES_ALIGN_TYPE_TEMPLATE(1); +CERES_ALIGN_TYPE_TEMPLATE(2); +CERES_ALIGN_TYPE_TEMPLATE(4); +CERES_ALIGN_TYPE_TEMPLATE(8); +CERES_ALIGN_TYPE_TEMPLATE(16); +CERES_ALIGN_TYPE_TEMPLATE(32); +CERES_ALIGN_TYPE_TEMPLATE(64); +CERES_ALIGN_TYPE_TEMPLATE(128); +CERES_ALIGN_TYPE_TEMPLATE(256); +CERES_ALIGN_TYPE_TEMPLATE(512); +CERES_ALIGN_TYPE_TEMPLATE(1024); +CERES_ALIGN_TYPE_TEMPLATE(2048); +CERES_ALIGN_TYPE_TEMPLATE(4096); +CERES_ALIGN_TYPE_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 CERES_ALIGN_TYPE_TEMPLATE -#undef BASE_PORT_H_ALIGNTYPE_TEMPLATE -#undef BASE_PORT_H_ALIGN_ATTRIBUTE +#define CERES_ALIGNED_CHAR_ARRAY(T, Size) \ + typename AlignType<CERES_ALIGN_OF(T), sizeof(T) * Size>::result -#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) +#endif // !defined(CERES_ALIGN_ATTRIBUTE) -#undef BASE_PORT_H_ALIGNTYPE_TEMPLATE -#undef BASE_PORT_H_ALIGN_ATTRIBUTE - -#endif // ALIGNED_CHAR_ARRAY +#endif // CERES_ALIGNED_CHAR_ARRAY template <typename Type> class ManualConstructor { @@ -203,10 +192,10 @@ class ManualConstructor { } private: - ALIGNED_CHAR_ARRAY(Type, 1) space_; + CERES_ALIGNED_CHAR_ARRAY(Type, 1) space_; }; -#undef ALIGNED_CHAR_ARRAY +#undef CERES_ALIGNED_CHAR_ARRAY } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/port.h b/extern/libmv/third_party/ceres/include/ceres/internal/port.h index 9a3e5cced58..a9fe247cef5 100644 --- a/extern/libmv/third_party/ceres/include/ceres/internal/port.h +++ b/extern/libmv/third_party/ceres/include/ceres/internal/port.h @@ -31,6 +31,8 @@ #ifndef CERES_PUBLIC_INTERNAL_PORT_H_ #define CERES_PUBLIC_INTERNAL_PORT_H_ +#include <string> + namespace ceres { // It is unfortunate that this import of the entire standard namespace is @@ -39,6 +41,10 @@ namespace ceres { // things outside of the Ceres optimization package. using namespace std; +// This is necessary to properly handle the case that there is a different +// "string" implementation in the global namespace. +using std::string; + } // namespace ceres #endif // CERES_PUBLIC_INTERNAL_PORT_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 index 88da992d0c5..29157d380f2 100644 --- a/extern/libmv/third_party/ceres/include/ceres/iteration_callback.h +++ b/extern/libmv/third_party/ceres/include/ceres/iteration_callback.h @@ -28,8 +28,9 @@ // // 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. +// When an iteration callback is specified, Ceres calls the callback +// after each minimizer step (if the minimizer has not converged) and +// passes it an IterationSummary object, defined below. #ifndef CERES_PUBLIC_ITERATION_CALLBACK_H_ #define CERES_PUBLIC_ITERATION_CALLBACK_H_ @@ -44,7 +45,15 @@ struct IterationSummary { // Current iteration number. int32 iteration; + // Step was numerically valid, i.e., all values are finite and the + // step reduces the value of the linearized model. + // + // Note: step_is_valid is false when iteration = 0. + bool step_is_valid; + // Whether or not the algorithm made progress in this iteration. + // + // Note: step_is_successful is false when iteration = 0. bool step_is_successful; // Value of the objective function. @@ -66,9 +75,10 @@ struct IterationSummary { // 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; + // Size of the trust region at the end of the current iteration. For + // the Levenberg-Marquardt algorithm, the regularization parameter + // mu = 1.0 / trust_region_radius. + double trust_region_radius; // For the inexact step Levenberg-Marquardt algorithm, this is the // relative accuracy with which the Newton(LM) step is solved. This @@ -81,13 +91,15 @@ struct IterationSummary { // 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 minimizer loop in the current + // iteration. + double iteration_time_in_seconds; + + // Time (in seconds) spent inside the trust region step solver. + double step_solver_time_in_seconds; - // Time (in seconds) spent inside the linear least squares solver. - int linear_solver_time_sec; + // Time (in seconds) since the user called Solve(). + double cumulative_time_in_seconds; }; // Interface for specifying callbacks that are executed at the end of @@ -133,7 +145,7 @@ struct IterationSummary { // summary.gradient_max_norm, // summary.step_norm, // summary.relative_decrease, -// summary.mu, +// summary.trust_region_radius, // summary.eta, // summary.linear_solver_iterations); // if (log_to_stdout_) { diff --git a/extern/libmv/third_party/ceres/include/ceres/jet.h b/extern/libmv/third_party/ceres/include/ceres/jet.h index a37870210f1..96e2256fd02 100644 --- a/extern/libmv/third_party/ceres/include/ceres/jet.h +++ b/extern/libmv/third_party/ceres/include/ceres/jet.h @@ -162,16 +162,7 @@ #include <string> #include "Eigen/Core" - -// Visual Studio 2012 or older version -#if defined(_MSC_VER) && _MSC_VER <= 1700 -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 +#include "ceres/fpclassify.h" namespace ceres { @@ -184,7 +175,9 @@ struct Jet { // (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() {} + Jet() : a() { + v.setZero(); + } // Constructor from scalar: a + 0. explicit Jet(const T& value) { @@ -199,18 +192,6 @@ struct Jet { 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; @@ -232,8 +213,25 @@ struct Jet { return *this; } - T a; // The scalar part. - Eigen::Matrix<T, N, 1> v; // The infinitesimal part. + // The scalar part. + T a; + + // The infinitesimal part. + // + // Note the Eigen::DontAlign bit is needed here because this object + // gets allocated on the stack and as part of other arrays and + // structs. Forcing the right alignment there is the source of much + // pain and suffering. Even if that works, passing Jets around to + // functions by value has problems because the C++ ABI does not + // guarantee alignment for function arguments. + // + // Setting the DontAlign bit prevents Eigen from using SSE for the + // various operations on Jets. This is a small performance penalty + // since the AutoDiff code will still expose much of the code as + // statically sized loops to the compiler. But given the subtle + // issues that arise due to alignment, especially when dealing with + // multiple platforms, it seems to be a trade off worth making. + Eigen::Matrix<T, N, 1, Eigen::DontAlign> v; }; // Unary + @@ -411,10 +409,6 @@ 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); } @@ -492,22 +486,23 @@ Jet<T, N> asin(const Jet<T, N>& f) { } // 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. +// 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 IsInfinite, the answer is less +// clear. This takes a "any" approach for IsNaN and IsInfinite 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 IsInfinite 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)) { +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])) { + if (!IsFinite(f.v[i])) { return false; } } @@ -516,12 +511,12 @@ bool isfinite(const Jet<T, N>& f) { // 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)) { +bool IsInfinite(const Jet<T, N>& f) { + if (IsInfinite(f.a)) { return true; } for (int i = 0; i < N; i++) { - if (isinf(f.v[i])) { + if (IsInfinite(f.v[i])) { return true; } } @@ -530,12 +525,12 @@ bool isinf(const Jet<T, N>& f) { // 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)) { +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])) { + if (IsNaN(f.v[i])) { return true; } } @@ -544,12 +539,12 @@ bool isnan(const Jet<T, N>& f) { // 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)) { +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])) { + if (!IsNormal(f.v[i])) { return false; } } @@ -650,78 +645,6 @@ 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 { diff --git a/extern/libmv/third_party/ceres/include/ceres/loss_function.h b/extern/libmv/third_party/ceres/include/ceres/loss_function.h index 81add02cdee..0c0ceaaecd0 100644 --- a/extern/libmv/third_party/ceres/include/ceres/loss_function.h +++ b/extern/libmv/third_party/ceres/include/ceres/loss_function.h @@ -175,6 +175,7 @@ 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. @@ -190,6 +191,7 @@ 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_; @@ -206,6 +208,7 @@ 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_; @@ -213,6 +216,78 @@ class CauchyLoss : public LossFunction { const double c_; }; +// Loss that is capped beyond a certain level using the arc-tangent function. +// The scaling parameter 'a' determines the level where falloff occurs. +// For costs much smaller than 'a', the loss function is linear and behaves like +// TrivialLoss, and for values much larger than 'a' the value asymptotically +// approaches the constant value of a * PI / 2. +// +// rho(s) = a atan(s / a). +// +// At s = 0: rho = [0, 1, 0]. +class ArctanLoss : public LossFunction { + public: + explicit ArctanLoss(double a) : a_(a), b_(1 / (a * a)) { } + virtual void Evaluate(double, double*) const; + + private: + const double a_; + // b = 1 / a^2. + const double b_; +}; + +// Loss function that maps to approximately zero cost in a range around the +// origin, and reverts to linear in error (quadratic in cost) beyond this range. +// The tolerance parameter 'a' sets the nominal point at which the +// transition occurs, and the transition size parameter 'b' sets the nominal +// distance over which most of the transition occurs. Both a and b must be +// greater than zero, and typically b will be set to a fraction of a. +// The slope rho'[s] varies smoothly from about 0 at s <= a - b to +// about 1 at s >= a + b. +// +// The term is computed as: +// +// rho(s) = b log(1 + exp((s - a) / b)) - c0. +// +// where c0 is chosen so that rho(0) == 0 +// +// c0 = b log(1 + exp(-a / b) +// +// This has the following useful properties: +// +// rho(s) == 0 for s = 0 +// rho'(s) ~= 0 for s << a - b +// rho'(s) ~= 1 for s >> a + b +// rho''(s) > 0 for all s +// +// In addition, all derivatives are continuous, and the curvature is +// concentrated in the range a - b to a + b. +// +// At s = 0: rho = [0, ~0, ~0]. +class TolerantLoss : public LossFunction { + public: + explicit TolerantLoss(double a, double b); + virtual void Evaluate(double, double*) const; + + private: + const double a_, b_, c_; +}; + +// Composition of two loss functions. The error is the result of first +// evaluating g followed by f to yield the composition f(g(s)). +// The loss functions must not be NULL. +class ComposedLoss : public LossFunction { + public: + explicit ComposedLoss(const LossFunction* f, Ownership ownership_f, + const LossFunction* g, Ownership ownership_g); + virtual ~ComposedLoss(); + virtual void Evaluate(double, double*) const; + + private: + internal::scoped_ptr<const LossFunction> f_, g_; + const Ownership ownership_f_, ownership_g_; +}; + // 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 @@ -249,7 +324,7 @@ class ScaledLoss : public LossFunction { internal::scoped_ptr<const LossFunction> rho_; const double a_; const Ownership ownership_; - DISALLOW_COPY_AND_ASSIGN(ScaledLoss); + CERES_DISALLOW_COPY_AND_ASSIGN(ScaledLoss); }; // Sometimes after the optimization problem has been constructed, we @@ -314,7 +389,7 @@ class LossFunctionWrapper : public LossFunction { private: internal::scoped_ptr<const LossFunction> rho_; Ownership ownership_; - DISALLOW_COPY_AND_ASSIGN(LossFunctionWrapper); + CERES_DISALLOW_COPY_AND_ASSIGN(LossFunctionWrapper); }; } // namespace ceres 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 index bbaefca5b6c..8544e44d0bc 100644 --- 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 @@ -93,11 +93,13 @@ struct Differencer { using Eigen::Map; using Eigen::Matrix; using Eigen::RowMajor; + using Eigen::ColMajor; 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; + typedef Matrix<double, num_residuals, parameter_block_size, + (parameter_block_size == 1 && + num_residuals > 1) ? ColMajor : RowMajor> JacobianMatrix; Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block], num_residuals, diff --git a/extern/libmv/third_party/ceres/include/ceres/problem.h b/extern/libmv/third_party/ceres/include/ceres/problem.h index 0ca61006bdb..2b08c6723e8 100644 --- a/extern/libmv/third_party/ceres/include/ceres/problem.h +++ b/extern/libmv/third_party/ceres/include/ceres/problem.h @@ -50,13 +50,13 @@ namespace ceres { class CostFunction; class LossFunction; class LocalParameterization; +class Solver; 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 @@ -255,9 +255,9 @@ class Problem { int NumResiduals() const; private: - friend class internal::SolverImpl; + friend class Solver; internal::scoped_ptr<internal::ProblemImpl> problem_impl_; - DISALLOW_COPY_AND_ASSIGN(Problem); + CERES_DISALLOW_COPY_AND_ASSIGN(Problem); }; } // namespace ceres diff --git a/extern/libmv/third_party/ceres/include/ceres/rotation.h b/extern/libmv/third_party/ceres/include/ceres/rotation.h index e4227e78b9a..0d8a390d5d1 100644 --- a/extern/libmv/third_party/ceres/include/ceres/rotation.h +++ b/extern/libmv/third_party/ceres/include/ceres/rotation.h @@ -47,6 +47,7 @@ #include <algorithm> #include <cmath> +#include "glog/logging.h" namespace ceres { @@ -145,18 +146,11 @@ 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& 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. @@ -183,16 +177,35 @@ inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { 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; + const T& q1 = quaternion[1]; + const T& q2 = quaternion[2]; + const T& q3 = quaternion[3]; + const T sin_squared_theta = 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; + if (sin_squared_theta > T(0.0)) { + const T sin_theta = sqrt(sin_squared_theta); + const T& cos_theta = quaternion[0]; + + // If cos_theta is negative, theta is greater than pi/2, which + // means that angle for the angle_axis vector which is 2 * theta + // would be greater than pi. + // + // While this will result in the correct rotation, it does not + // result in a normalized angle-axis vector. + // + // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi, + // which is equivalent saying + // + // theta - pi = atan(sin(theta - pi), cos(theta - pi)) + // = atan(-sin(theta), -cos(theta)) + // + const T two_theta = + T(2.0) * ((cos_theta < 0.0) + ? atan2(-sin_theta, -cos_theta) + : atan2(sin_theta, cos_theta)); + const T k = two_theta / sin_theta; angle_axis[0] = q1 * k; angle_axis[1] = q2 * k; angle_axis[2] = q3 * k; @@ -259,7 +272,7 @@ inline void RotationMatrixToAngleAxis(const T * R, T * angle_axis) { // Case 2: theta ~ 0, means sin(theta) ~ theta to a good // approximation. - if (costheta > 0) { + if (costheta > 0.0) { const T kHalf = T(0.5); for (int i = 0; i < 3; ++i) { angle_axis[i] *= kHalf; @@ -284,8 +297,8 @@ inline void RotationMatrixToAngleAxis(const T * R, T * angle_axis) { // 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))) { + if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) || + ((sintheta > 0.0) && (angle_axis[i] < 0.0))) { angle_axis[i] = -angle_axis[i]; } } @@ -334,7 +347,8 @@ 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 double kPi = 3.14159265358979323846; + const T degrees_to_radians(kPi / 180.0); const T pitch(euler[0] * degrees_to_radians); const T roll(euler[1] * degrees_to_radians); @@ -517,10 +531,4 @@ void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { } // 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/solver.h b/extern/libmv/third_party/ceres/include/ceres/solver.h index bd669272023..31d5e8d7987 100644 --- a/extern/libmv/third_party/ceres/include/ceres/solver.h +++ b/extern/libmv/third_party/ceres/include/ceres/solver.h @@ -34,10 +34,10 @@ #include <cmath> #include <string> #include <vector> - -#include "ceres/iteration_callback.h" +#include "ceres/crs_matrix.h" #include "ceres/internal/macros.h" #include "ceres/internal/port.h" +#include "ceres/iteration_callback.h" #include "ceres/types.h" namespace ceres { @@ -57,24 +57,47 @@ class Solver { struct Options { // Default constructor that sets up a generic sparse problem. Options() { - minimizer_type = LEVENBERG_MARQUARDT; + trust_region_strategy_type = LEVENBERG_MARQUARDT; + dogleg_type = TRADITIONAL_DOGLEG; + use_nonmonotonic_steps = false; + max_consecutive_nonmonotonic_steps = 5; max_num_iterations = 50; - max_solver_time_sec = 1.0e9; + max_solver_time_in_seconds = 1e9; num_threads = 1; - tau = 1e-4; + initial_trust_region_radius = 1e4; + max_trust_region_radius = 1e16; + min_trust_region_radius = 1e-32; min_relative_decrease = 1e-3; + lm_min_diagonal = 1e-6; + lm_max_diagonal = 1e32; + max_num_consecutive_invalid_steps = 5; function_tolerance = 1e-6; gradient_tolerance = 1e-10; parameter_tolerance = 1e-8; -#ifndef CERES_NO_SUITESPARSE - linear_solver_type = SPARSE_NORMAL_CHOLESKY; -#else + +#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) linear_solver_type = DENSE_QR; -#endif // CERES_NO_SUITESPARSE +#else + linear_solver_type = SPARSE_NORMAL_CHOLESKY; +#endif + preconditioner_type = JACOBI; + + sparse_linear_algebra_library = SUITE_SPARSE; +#if defined(CERES_NO_SUITESPARSE) && !defined(CERES_NO_CXSPARSE) + sparse_linear_algebra_library = CX_SPARSE; +#endif + num_linear_solver_threads = 1; num_eliminate_blocks = 0; ordering_type = NATURAL; + +#if defined(CERES_NO_SUITESPARSE) + use_block_amd = false; +#else + use_block_amd = true; +#endif + linear_solver_min_num_iterations = 1; linear_solver_max_num_iterations = 500; eta = 1e-1; @@ -82,10 +105,13 @@ class Solver { logging_type = PER_MINIMIZER_ITERATION; minimizer_progress_to_stdout = false; return_initial_residuals = false; + return_initial_gradient = false; + return_initial_jacobian = false; return_final_residuals = false; + return_final_gradient = false; + return_final_jacobian = 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; @@ -94,27 +120,78 @@ class Solver { // Minimizer options ---------------------------------------- - MinimizerType minimizer_type; + TrustRegionStrategyType trust_region_strategy_type; + + // Type of dogleg strategy to use. + DoglegType dogleg_type; + + // The classical trust region methods are descent methods, in that + // they only accept a point if it strictly reduces the value of + // the objective function. + // + // Relaxing this requirement allows the algorithm to be more + // efficient in the long term at the cost of some local increase + // in the value of the objective function. + // + // This is because allowing for non-decreasing objective function + // values in a princpled manner allows the algorithm to "jump over + // boulders" as the method is not restricted to move into narrow + // valleys while preserving its convergence properties. + // + // Setting use_nonmonotonic_steps to true enables the + // non-monotonic trust region algorithm as described by Conn, + // Gould & Toint in "Trust Region Methods", Section 10.1. + // + // The parameter max_consecutive_nonmonotonic_steps controls the + // window size used by the step selection algorithm to accept + // non-monotonic steps. + // + // Even though the value of the objective function may be larger + // than the minimum value encountered over the course of the + // optimization, the final parameters returned to the user are the + // ones corresponding to the minimum cost over all iterations. + bool use_nonmonotonic_steps; + int max_consecutive_nonmonotonic_steps; // 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; + double max_solver_time_in_seconds; // 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; + // Trust region minimizer settings. + double initial_trust_region_radius; + double max_trust_region_radius; + + // Minimizer terminates when the trust region radius becomes + // smaller than this value. + double min_trust_region_radius; - // For trust region methods, this is lower threshold for the - // relative decrease before a step is accepted. + // Lower bound for the relative decrease before a step is + // accepted. double min_relative_decrease; + // For the Levenberg-Marquadt algorithm, the scaled diagonal of + // the normal equations J'J is used to control the size of the + // trust region. Extremely small and large values along the + // diagonal can make this regularization scheme + // fail. lm_max_diagonal and lm_min_diagonal, clamp the values of + // diag(J'J) from above and below. In the normal course of + // operation, the user should not have to modify these parameters. + double lm_min_diagonal; + double lm_max_diagonal; + + // Sometimes due to numerical conditioning problems or linear + // solver flakiness, the trust region strategy may return a + // numerically invalid step that can be fixed by reducing the + // trust region size. So the TrustRegionMinimizer allows for a few + // successive invalid steps before it declares NUMERICAL_FAILURE. + int max_num_consecutive_invalid_steps; + // Minimizer terminates when // // (new_cost - old_cost) < function_tolerance * old_cost; @@ -141,6 +218,12 @@ class Solver { // Type of preconditioner to use with the iterative linear solvers. PreconditionerType preconditioner_type; + // Ceres supports using multiple sparse linear algebra libraries + // for sparse matrix ordering and factorizations. Currently, + // SUITE_SPARSE and CX_SPARSE are the valid choices, depending on + // whether they are linked into Ceres at build time. + SparseLinearAlgebraLibraryType sparse_linear_algebra_library; + // Number of threads used by Ceres to solve the Newton // step. Currently only the SPARSE_SCHUR solver is capable of // using this setting. @@ -170,6 +253,19 @@ class Solver { // non-empty. vector<double*> ordering; + // By virtue of the modeling layer in Ceres being block oriented, + // all the matrices used by Ceres are also block oriented. When + // doing sparse direct factorization of these matrices (for + // SPARSE_NORMAL_CHOLESKY, SPARSE_SCHUR and ITERATIVE in + // conjunction with CLUSTER_TRIDIAGONAL AND CLUSTER_JACOBI + // preconditioners), the fill-reducing ordering algorithms can + // either be run on the block or the scalar form of these matrices. + // Running it on the block form exposes more of the super-nodal + // structure of the matrix to the factorization routines. Setting + // this parameter to true runs the ordering algorithms in block + // form. Currently this option only makes sense with + // sparse_linear_algebra_library = SUITE_SPARSE. + bool use_block_amd; // Minimum number of iterations for which the linear solver should // run, even if the convergence criterion is satisfied. @@ -206,7 +302,12 @@ class Solver { bool minimizer_progress_to_stdout; bool return_initial_residuals; + bool return_initial_gradient; + bool return_initial_jacobian; + bool return_final_residuals; + bool return_final_gradient; + bool return_final_jacobian; // List of iterations at which the optimizer should dump the // linear least squares problem to disk. Useful for testing and @@ -217,15 +318,6 @@ class Solver { 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 @@ -273,16 +365,25 @@ class Solver { 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 + // Minimizer. An iteration may terminate midway, either due to + // numerical failures or because one of the convergence tests has + // been satisfied. In this case none of the callbacks are + // executed. + + // Callbacks 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; + + // If non-empty, a summary of the execution of the solver is + // recorded to this file. + string solver_log; }; struct Summary { @@ -313,20 +414,74 @@ class Solver { // 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. + // Vectors of residuals before and after the optimization. The + // entries of these vectors are in the order in which + // ResidualBlocks were added to the Problem object. + // + // Whether the residual vectors are populated with values is + // controlled by Solver::Options::return_initial_residuals and + // Solver::Options::return_final_residuals respectively. vector<double> initial_residuals; vector<double> final_residuals; + // Gradient vectors, before and after the optimization. The rows + // are in the same order in which the ParameterBlocks were added + // to the Problem object. + // + // NOTE: Since AddResidualBlock adds ParameterBlocks to the + // Problem automatically if they do not already exist, if you wish + // to have explicit control over the ordering of the vectors, then + // use Problem::AddParameterBlock to explicitly add the + // ParameterBlocks in the order desired. + // + // Whether the vectors are populated with values is controlled by + // Solver::Options::return_initial_gradient and + // Solver::Options::return_final_gradient respectively. + vector<double> initial_gradient; + vector<double> final_gradient; + + // Jacobian matrices before and after the optimization. The rows + // of these matrices are in the same order in which the + // ResidualBlocks were added to the Problem object. The columns + // are in the same order in which the ParameterBlocks were added + // to the Problem object. + // + // NOTE: Since AddResidualBlock adds ParameterBlocks to the + // Problem automatically if they do not already exist, if you wish + // to have explicit control over the column ordering of the + // matrix, then use Problem::AddParameterBlock to explicitly add + // the ParameterBlocks in the order desired. + // + // The Jacobian matrices are stored as compressed row sparse + // matrices. Please see ceres/crs_matrix.h for more details of the + // format. + // + // Whether the Jacboan matrices are populated with values is + // controlled by Solver::Options::return_initial_jacobian and + // Solver::Options::return_final_jacobian respectively. + CRSMatrix initial_jacobian; + CRSMatrix final_jacobian; + vector<IterationSummary> iterations; int num_successful_steps; int num_unsuccessful_steps; + // When the user calls Solve, before the actual optimization + // occurs, Ceres performs a number of preprocessing steps. These + // include error checks, memory allocations, and reorderings. This + // time is accounted for as preprocessing time. double preprocessor_time_in_seconds; + + // Time spent in the TrustRegionMinimizer. double minimizer_time_in_seconds; + + // After the Minimizer is finished, some time is spent in + // re-evaluating residuals etc. This time is accounted for in the + // postprocessor time. + double postprocessor_time_in_seconds; + + // Some total of all time spent inside Ceres when Solve is called. double total_time_in_seconds; // Preprocessor summary. @@ -354,6 +509,10 @@ class Solver { PreconditionerType preconditioner_type; OrderingType ordering_type; + + TrustRegionStrategyType trust_region_strategy_type; + DoglegType dogleg_type; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library; }; // Once a least squares problem has been built, this function takes diff --git a/extern/libmv/third_party/ceres/include/ceres/types.h b/extern/libmv/third_party/ceres/include/ceres/types.h index a30c79029ac..3980885b53c 100644 --- a/extern/libmv/third_party/ceres/include/ceres/types.h +++ b/extern/libmv/third_party/ceres/include/ceres/types.h @@ -59,14 +59,18 @@ enum LinearSolverType { // 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 Cholesky solver; based + // on Eigen. + DENSE_NORMAL_CHOLESKY, // Solve the normal equations using a dense QR solver; based on // Eigen. DENSE_QR, + // Solve the normal equations using a sparse cholesky solver; requires + // SuiteSparse or CXSparse. + SPARSE_NORMAL_CHOLESKY, + // Specialized solvers, specific to problems with a generalized // bi-partitite structure. @@ -110,6 +114,15 @@ enum PreconditionerType { CLUSTER_TRIDIAGONAL }; +enum SparseLinearAlgebraLibraryType { + // High performance sparse Cholesky factorization and approximate + // minimum degree ordering. + SUITE_SPARSE, + + // A lightweight replacment for SuiteSparse. + CX_SPARSE +}; + enum LinearSolverTerminationType { // Termination criterion was met. For factorization based solvers // the tolerance is assumed to be zero. Any user provided values are @@ -149,8 +162,47 @@ enum LoggingType { PER_MINIMIZER_ITERATION }; -enum MinimizerType { - LEVENBERG_MARQUARDT +// Ceres supports different strategies for computing the trust region +// step. +enum TrustRegionStrategyType { + // The default trust region strategy is to use the step computation + // used in the Levenberg-Marquardt algorithm. For more details see + // levenberg_marquardt_strategy.h + LEVENBERG_MARQUARDT, + + // Powell's dogleg algorithm interpolates between the Cauchy point + // and the Gauss-Newton step. It is particularly useful if the + // LEVENBERG_MARQUARDT algorithm is making a large number of + // unsuccessful steps. For more details see dogleg_strategy.h. + // + // NOTES: + // + // 1. This strategy has not been experimented with or tested as + // extensively as LEVENBERG_MARQUARDT, and therefore it should be + // considered EXPERIMENTAL for now. + // + // 2. For now this strategy should only be used with exact + // factorization based linear solvers, i.e., SPARSE_SCHUR, + // DENSE_SCHUR, DENSE_QR and SPARSE_NORMAL_CHOLESKY. + DOGLEG +}; + +// Ceres supports two different dogleg strategies. +// The "traditional" dogleg method by Powell and the +// "subspace" method described in +// R. H. Byrd, R. B. Schnabel, and G. A. Shultz, +// "Approximate solution of the trust region problem by minimization +// over two-dimensional subspaces", Mathematical Programming, +// 40 (1988), pp. 247--263 +enum DoglegType { + // The traditional approach constructs a dogleg path + // consisting of two line segments and finds the furthest + // point on that path that is still inside the trust region. + TRADITIONAL_DOGLEG, + + // The subspace approach finds the exact minimum of the model + // constrained to the subspace spanned by the dogleg path. + SUBSPACE_DOGLEG }; enum SolverTerminationType { @@ -246,11 +298,15 @@ enum DimensionType { const char* LinearSolverTypeToString(LinearSolverType type); const char* PreconditionerTypeToString(PreconditionerType type); +const char* SparseLinearAlgebraLibraryTypeToString( + SparseLinearAlgebraLibraryType type); const char* LinearSolverTerminationTypeToString( LinearSolverTerminationType type); const char* OrderingTypeToString(OrderingType type); const char* SolverTerminationTypeToString(SolverTerminationType type); - +const char* SparseLinearAlgebraLibraryTypeToString( + SparseLinearAlgebraLibraryType type); +const char* TrustRegionStrategyTypeToString( TrustRegionStrategyType type); bool IsSchurType(LinearSolverType type); } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.h b/extern/libmv/third_party/ceres/internal/ceres/array_utils.cc index d00bb9095be..673baa4f70f 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.h +++ b/extern/libmv/third_party/ceres/internal/ceres/array_utils.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// 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 @@ -27,39 +27,41 @@ // 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/array_utils.h" -#include "ceres/minimizer.h" -#include "ceres/solver.h" +#include <cmath> +#include <cstddef> +#include "ceres/fpclassify.h" namespace ceres { namespace internal { -class Evaluator; -class LinearSolver; +// 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. +const double kImpossibleValue = 1e302; -class LevenbergMarquardt : public Minimizer { - public: - virtual ~LevenbergMarquardt(); +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; +} - virtual void Minimize(const Minimizer::Options& options, - Evaluator* evaluator, - LinearSolver* linear_solver, - const double* initial_parameters, - double* final_parameters, - Solver::Summary* summary); -}; +void InvalidateArray(const int size, double* x) { + if (x != NULL) { + for (int i = 0; i < size; ++i) { + x[i] = kImpossibleValue; + } + } +} } // namespace internal } // namespace ceres - -#endif // CERES_INTERNAL_LEVENBERG_MARQUARDT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/array_utils.h b/extern/libmv/third_party/ceres/internal/ceres/array_utils.h new file mode 100644 index 00000000000..99cc8d8ebbf --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/array_utils.h @@ -0,0 +1,65 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) +// +// Utility routines for validating arrays. +// +// These are useful for detecting two common class of errors. +// +// 1. Uninitialized memory - where the user for some reason did not +// compute part of an array, but the code expects it. +// +// 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_ARRAY_UTILS_H_ +#define CERES_INTERNAL_ARRAY_UTILS_H_ + +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +// 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); + +extern const double kImpossibleValue; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_ARRAY_UTILS_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 index 05e63eb560b..9edc4fa23bd 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.cc @@ -40,16 +40,26 @@ namespace ceres { namespace internal { -void BlockEvaluatePreparer::Init(int** jacobian_layout) { +void BlockEvaluatePreparer::Init(int const* const* jacobian_layout, + int max_derivatives_per_residual_block) { jacobian_layout_ = jacobian_layout; + scratch_evaluate_preparer_.Init(max_derivatives_per_residual_block); } // 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** jacobians) { + // If the overall jacobian is not available, use the scratch space. + if (jacobian == NULL) { + scratch_evaluate_preparer_.Prepare(residual_block, + residual_block_index, + jacobian, + jacobians); + return; + } + double* jacobian_values = down_cast<BlockSparseMatrix*>(jacobian)->mutable_values(); 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 index a7869311e6e..354acc031f4 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.h +++ b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.h @@ -36,6 +36,8 @@ #ifndef CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ #define CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ +#include "ceres/scratch_evaluate_preparer.h" + namespace ceres { namespace internal { @@ -47,18 +49,26 @@ class BlockEvaluatePreparer { // 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); + void Init(int const* const* jacobian_layout, + int max_derivatives_per_residual_block); // EvaluatePreparer interface - // Point the jacobian blocks directly into the block sparse matrix. + // Point the jacobian blocks directly into the block sparse matrix, if + // jacobian is non-null. Otherwise, uses an internal per-thread buffer to + // store the jacobians temporarily. void Prepare(const ResidualBlock* residual_block, int residual_block_index, SparseMatrix* jacobian, - double** jacobians) const; + double** jacobians); private: int const* const* jacobian_layout_; + + // For the case that the overall jacobian is not available, but the + // individual jacobians are requested, use a pass-through scratch evaluate + // preparer. + ScratchEvaluatePreparer scratch_evaluate_preparer_; }; } // namespace internal 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 index 1a5001f9c71..474c37f7ca4 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc @@ -40,11 +40,10 @@ namespace ceres { namespace internal { -BlockJacobiPreconditioner::BlockJacobiPreconditioner( - const LinearOperator& A) - : block_structure_( - *(down_cast<const BlockSparseMatrix*>(&A)->block_structure())), - num_rows_(A.num_rows()) { +BlockJacobiPreconditioner::BlockJacobiPreconditioner(const LinearOperator& A) + : num_rows_(A.num_rows()), + block_structure_( + *(down_cast<const BlockSparseMatrix*>(&A)->block_structure())) { // Calculate the amount of storage needed. int storage_needed = 0; for (int c = 0; c < block_structure_.cols.size(); ++c) { 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 index 52a58bb43a6..f90c350cc80 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.cc @@ -136,9 +136,12 @@ BlockJacobianWriter::BlockJacobianWriter(const Evaluator::Options& options, // makes the final Write() a nop. BlockEvaluatePreparer* BlockJacobianWriter::CreateEvaluatePreparers( int num_threads) { + int max_derivatives_per_residual_block = + program_->MaxDerivativesPerResidualBlock(); + BlockEvaluatePreparer* preparers = new BlockEvaluatePreparer[num_threads]; for (int i = 0; i < num_threads; i++) { - preparers[i].Init(&jacobian_layout_[0]); + preparers[i].Init(&jacobian_layout_[0], max_derivatives_per_residual_block); } return preparers; } 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 index 2afaf5e2ea2..aedfc745f22 100644 --- 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 @@ -31,9 +31,9 @@ #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" +#include "glog/logging.h" namespace ceres { namespace internal { 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 index 3a0096209f7..9f27a4c30f3 100644 --- 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 @@ -89,7 +89,7 @@ class BlockRandomAccessDenseMatrix : public BlockRandomAccessMatrix { vector<int> block_layout_; scoped_array<double> values_; - DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessDenseMatrix); + CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessDenseMatrix); }; } // namespace internal 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 index f398af3be87..b76cb78b160 100644 --- 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 @@ -77,7 +77,7 @@ namespace internal { // // if (cell != NULL) { // MatrixRef m(cell->values, row_stride, col_stride); -// MutexLock l(&cell->m); +// CeresMutexLock l(&cell->m); // m.block(row, col, row_block_size, col_block_size) = ... // } 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 index c496fcd13de..f789436364a 100644 --- 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 @@ -34,12 +34,12 @@ #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/mutex.h" +#include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" +#include "glog/logging.h" namespace ceres { namespace internal { 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 index 12613c3daa0..48a00437cf6 100644 --- 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 @@ -38,6 +38,7 @@ #include "ceres/block_random_access_matrix.h" #include "ceres/collections_port.h" #include "ceres/triplet_sparse_matrix.h" +#include "ceres/integral_types.h" #include "ceres/internal/macros.h" #include "ceres/internal/port.h" #include "ceres/internal/scoped_ptr.h" @@ -84,11 +85,11 @@ class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } private: - long int IntPairToLong(int a, int b) { + int64 IntPairToLong(int a, int b) { return a * kMaxRowBlocks + b; } - const int kMaxRowBlocks; + const int64 kMaxRowBlocks; // row/column block sizes. const vector<int> blocks_; @@ -100,7 +101,8 @@ class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { // The underlying matrix object which actually stores the cells. scoped_ptr<TripletSparseMatrix> tsm_; - DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessSparseMatrix); + friend class BlockRandomAccessSparseMatrixTest; + CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessSparseMatrix); }; } // namespace internal 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 index 7dd395e2975..dbe5ec93ef0 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc @@ -33,11 +33,11 @@ #include <cstddef> #include <algorithm> #include <vector> -#include <glog/logging.h> #include "ceres/block_structure.h" +#include "ceres/internal/eigen.h" #include "ceres/matrix_proto.h" #include "ceres/triplet_sparse_matrix.h" -#include "ceres/internal/eigen.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -81,7 +81,7 @@ BlockSparseMatrix::BlockSparseMatrix( CHECK_NOTNULL(values_.get()); } -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS BlockSparseMatrix::BlockSparseMatrix(const SparseMatrixProto& outer_proto) { CHECK(outer_proto.has_block_matrix()); @@ -244,7 +244,7 @@ const CompressedRowBlockStructure* BlockSparseMatrix::block_structure() return block_structure_.get(); } -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS void BlockSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { outer_proto->Clear(); 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 index f71446e8f58..513d398c54d 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h @@ -74,7 +74,7 @@ class BlockSparseMatrixBase : public SparseMatrix { virtual const double* RowBlockValues(int row_block_index) const = 0; private: - DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrixBase); + CERES_DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrixBase); }; // This class implements the SparseMatrix interface for storing and @@ -96,7 +96,7 @@ class BlockSparseMatrix : public BlockSparseMatrixBase { explicit BlockSparseMatrix(CompressedRowBlockStructure* block_structure); // Construct a block sparse matrix from a protocol buffer. -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS explicit BlockSparseMatrix(const SparseMatrixProto& proto); #endif @@ -110,7 +110,7 @@ class BlockSparseMatrix : public BlockSparseMatrixBase { 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 +#ifndef CERES_NO_PROTOCOL_BUFFERS virtual void ToProto(SparseMatrixProto* proto) const; #endif virtual void ToTextFile(FILE* file) const; @@ -135,7 +135,7 @@ class BlockSparseMatrix : public BlockSparseMatrixBase { int num_nonzeros_; scoped_array<double> values_; scoped_ptr<CompressedRowBlockStructure> block_structure_; - DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrix); + CERES_DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrix); }; } // namespace internal diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc b/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc index 5add4f3b94d..e61131192af 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc @@ -38,7 +38,7 @@ bool CellLessThan(const Cell& lhs, const Cell& rhs) { return (lhs.block_id < rhs.block_id); } -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS void ProtoToBlockStructure(const BlockStructureProto &proto, CompressedRowBlockStructure *block_structure) { // Decode the column blocks. 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 index 53190ada6fc..d0dc1e670c2 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc @@ -31,11 +31,11 @@ #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/graph.h" #include "ceres/internal/macros.h" +#include "ceres/map_util.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -75,7 +75,7 @@ class CanonicalViewsClustering { 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); + CERES_DISALLOW_COPY_AND_ASSIGN(CanonicalViewsClustering); }; void ComputeCanonicalViewsClustering( diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h index dd36f99006b..877b4c4ceea 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h @@ -57,7 +57,7 @@ class CgnrSolver : public LinearSolver { private: const LinearSolver::Options options_; scoped_ptr<BlockJacobiPreconditioner> jacobi_preconditioner_; - DISALLOW_COPY_AND_ASSIGN(CgnrSolver); + CERES_DISALLOW_COPY_AND_ASSIGN(CgnrSolver); }; } // namespace internal diff --git a/extern/libmv/third_party/ceres/internal/ceres/collections_port.h b/extern/libmv/third_party/ceres/internal/ceres/collections_port.h index 9dff0efe245..c2fce9033cd 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/collections_port.h +++ b/extern/libmv/third_party/ceres/internal/ceres/collections_port.h @@ -33,31 +33,49 @@ #ifndef CERES_INTERNAL_COLLECTIONS_PORT_H_ #define CERES_INTERNAL_COLLECTIONS_PORT_H_ -#ifdef CERES_HASH_BOOST -#include <boost/tr1/unordered_map.hpp> -#include <boost/tr1/unordered_set.hpp> +#if defined(CERES_NO_TR1) +# include <map> +# include <set> #else -#if defined(_MSC_VER) && _MSC_VER <= 1700 -#include <unordered_map> -#include <unordered_set> -#else -#include <tr1/unordered_map> -#include <tr1/unordered_set> -#endif +# if defined(_MSC_VER) && _MSC_VER <= 1600 +# include <unordered_map> +# include <unordered_set> +# else +# include <tr1/unordered_map> +# include <tr1/unordered_set> +# endif #endif - #include <utility> #include "ceres/integral_types.h" #include "ceres/internal/port.h" +// Some systems don't have access to TR1. In that case, substitute the hash +// map/set with normal map/set. The price to pay is slightly slower speed for +// some operations. +#if defined(CERES_NO_TR1) + namespace ceres { namespace internal { template<typename K, typename V> -struct HashMap : tr1::unordered_map<K, V> {}; +struct HashMap : map<K, V> {}; template<typename K> -struct HashSet : tr1::unordered_set<K> {}; +struct HashSet : set<K> {}; + +} // namespace internal +} // namespace ceres + +#else + +namespace ceres { +namespace internal { + +template<typename K, typename V> +struct HashMap : std::tr1::unordered_map<K, V> {}; + +template<typename K> +struct HashSet : std::tr1::unordered_set<K> {}; #if defined(_WIN32) && !defined(__MINGW64__) && !defined(__MINGW32__) #define GG_LONGLONG(x) x##I64 @@ -124,11 +142,7 @@ CERES_HASH_NAMESPACE_START // Hasher for STL pairs. Requires hashers for both members to be defined. template<typename T> -#ifdef CERES_HASH_BOOST -struct hash { -#else struct hash<pair<T, T> > { -#endif size_t operator()(const pair<T, T>& p) const { size_t h1 = hash<T>()(p.first); size_t h2 = hash<T>()(p.second); @@ -148,4 +162,6 @@ struct hash<pair<T, T> > { CERES_HASH_NAMESPACE_END +#endif // CERES_NO_TR1 + #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 index aa883b7d353..912c4845441 100644 --- 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 @@ -130,8 +130,24 @@ SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { } row_pos += num_residuals; } - CHECK_EQ(num_jacobian_nonzeros, rows[total_num_residuals]); + + // Populate the row and column block vectors for use by block + // oriented ordering algorithms. This is useful when + // Solver::Options::use_block_amd = true. + const vector<ParameterBlock*>& parameter_blocks = program_->parameter_blocks(); + vector<int>& col_blocks = *(jacobian->mutable_col_blocks()); + col_blocks.resize(parameter_blocks.size()); + for (int i = 0; i < parameter_blocks.size(); ++i) { + col_blocks[i] = parameter_blocks[i]->LocalSize(); + } + + vector<int>& row_blocks = *(jacobian->mutable_row_blocks()); + row_blocks.resize(residual_blocks.size()); + for (int i = 0; i < residual_blocks.size(); ++i) { + row_blocks[i] = residual_blocks[i]->NumResiduals(); + } + return jacobian; } 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 index 95edf5396af..1b61468aaae 100644 --- 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 @@ -32,15 +32,22 @@ #include <algorithm> #include <vector> -#include "ceres/matrix_proto.h" +#include "ceres/crs_matrix.h" #include "ceres/internal/port.h" +#include "ceres/matrix_proto.h" namespace ceres { namespace internal { namespace { // Helper functor used by the constructor for reordering the contents -// of a TripletSparseMatrix. +// of a TripletSparseMatrix. This comparator assumes thay there are no +// duplicates in the pair of arrays rows and cols, i.e., there is no +// indices i and j (not equal to each other) s.t. +// +// rows[i] == rows[j] && cols[i] == cols[j] +// +// If this is the case, this functor will not be a StrictWeakOrdering. struct RowColLessThan { RowColLessThan(const int* rows, const int* cols) : rows(rows), cols(cols) { @@ -128,7 +135,7 @@ CompressedRowSparseMatrix::CompressedRowSparseMatrix( CHECK_EQ(num_nonzeros(), m.num_nonzeros()); } -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS CompressedRowSparseMatrix::CompressedRowSparseMatrix( const SparseMatrixProto& outer_proto) { CHECK(outer_proto.has_compressed_row_matrix()); @@ -241,7 +248,7 @@ void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { } } -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS void CompressedRowSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { CHECK_NOTNULL(outer_proto); @@ -330,5 +337,18 @@ void CompressedRowSparseMatrix::ToTextFile(FILE* file) const { } } +void CompressedRowSparseMatrix::ToCRSMatrix(CRSMatrix* matrix) const { + matrix->num_rows = num_rows(); + matrix->num_cols = num_cols(); + + matrix->rows.resize(matrix->num_rows + 1); + matrix->cols.resize(num_nonzeros()); + matrix->values.resize(num_nonzeros()); + + copy(rows_.get(), rows_.get() + matrix->num_rows + 1, matrix->rows.begin()); + copy(cols_.get(), cols_.get() + num_nonzeros(), matrix->cols.begin()); + copy(values_.get(), values_.get() + num_nonzeros(), matrix->values.begin()); +} + } // 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 index 9a39d28e111..6a9d82842e5 100644 --- 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 @@ -31,14 +31,19 @@ #ifndef CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ #define CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ +#include <vector> #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/internal/port.h" #include "ceres/types.h" namespace ceres { + +class CRSMatrix; + namespace internal { class SparseMatrixProto; @@ -52,7 +57,7 @@ class CompressedRowSparseMatrix : public SparseMatrix { // // We assume that m does not have any repeated entries. explicit CompressedRowSparseMatrix(const TripletSparseMatrix& m); -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS explicit CompressedRowSparseMatrix(const SparseMatrixProto& proto); #endif @@ -85,7 +90,7 @@ class CompressedRowSparseMatrix : public SparseMatrix { virtual void ScaleColumns(const double* scale); virtual void ToDenseMatrix(Matrix* dense_matrix) const; -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS virtual void ToProto(SparseMatrixProto* proto) const; #endif virtual void ToTextFile(FILE* file) const; @@ -103,6 +108,8 @@ class CompressedRowSparseMatrix : public SparseMatrix { // have the same number of columns as this matrix. void AppendRows(const CompressedRowSparseMatrix& m); + void ToCRSMatrix(CRSMatrix* matrix) const; + // Low level access methods that expose the structure of the matrix. const int* cols() const { return cols_.get(); } int* mutable_cols() { return cols_.get(); } @@ -110,6 +117,12 @@ class CompressedRowSparseMatrix : public SparseMatrix { const int* rows() const { return rows_.get(); } int* mutable_rows() { return rows_.get(); } + const vector<int>& row_blocks() const { return row_blocks_; } + vector<int>* mutable_row_blocks() { return &row_blocks_; } + + const vector<int>& col_blocks() const { return col_blocks_; } + vector<int>* mutable_col_blocks() { return &col_blocks_; } + private: scoped_array<int> cols_; scoped_array<int> rows_; @@ -117,10 +130,17 @@ class CompressedRowSparseMatrix : public SparseMatrix { int num_rows_; int num_cols_; - int max_num_nonzeros_; - DISALLOW_COPY_AND_ASSIGN(CompressedRowSparseMatrix); + // If the matrix has an underlying block structure, then it can also + // carry with it row and column block sizes. This is auxilliary and + // optional information for use by algorithms operating on the + // matrix. The class itself does not make use of this information in + // any way. + vector<int> row_blocks_; + vector<int> col_blocks_; + + CERES_DISALLOW_COPY_AND_ASSIGN(CompressedRowSparseMatrix); }; } // namespace internal 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 index ca80bfb9c9d..7322790f717 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/conditioned_cost_function.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/conditioned_cost_function.cc @@ -34,10 +34,10 @@ #include <cstddef> -#include <glog/logging.h> -#include "ceres/stl_util.h" #include "ceres/internal/eigen.h" +#include "ceres/stl_util.h" #include "ceres/types.h" +#include "glog/logging.h" 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 index 75f9e043fa5..ae8e8774709 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc @@ -41,18 +41,18 @@ #include <cmath> #include <cstddef> -#include <glog/logging.h> -#include "ceres/linear_operator.h" +#include "ceres/fpclassify.h" #include "ceres/internal/eigen.h" +#include "ceres/linear_operator.h" #include "ceres/types.h" -#include "ceres/jet.h" +#include "glog/logging.h" namespace ceres { namespace internal { namespace { bool IsZeroOrInfinity(double x) { - return ((x == 0.0) || (isinf(x))); + return ((x == 0.0) || (IsInfinite(x))); } // Constant used in the MATLAB implementation ~ 2 * eps. @@ -115,7 +115,7 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( for (summary.num_iterations = 1; summary.num_iterations < options_.max_num_iterations; ++summary.num_iterations) { - VLOG(2) << "cg iteration " << summary.num_iterations; + VLOG(3) << "cg iteration " << summary.num_iterations; // Apply preconditioner if (per_solve_options.preconditioner != NULL) { @@ -151,14 +151,14 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( A->RightMultiply(p.data(), q.data()); double pq = p.dot(q); - if ((pq <= 0) || isinf(pq)) { + if ((pq <= 0) || IsInfinite(pq)) { LOG(ERROR) << "Numerical failure. pq = " << pq; summary.termination_type = FAILURE; break; } double alpha = rho / pq; - if (isinf(alpha)) { + if (IsInfinite(alpha)) { LOG(ERROR) << "Numerical failure. alpha " << alpha; summary.termination_type = FAILURE; break; @@ -202,13 +202,13 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( // 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 + VLOG(3) << "Q termination: zeta " << zeta << " " << per_solve_options.q_tolerance; if (zeta < per_solve_options.q_tolerance) { summary.termination_type = TOLERANCE; @@ -218,7 +218,7 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( // Residual based termination. norm_r = r. norm(); - VLOG(2) << "R termination: norm_r " << norm_r + VLOG(3) << "R termination: norm_r " << norm_r << " " << tol_r; if (norm_r <= tol_r) { summary.termination_type = TOLERANCE; 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 index 57f99e31db7..b8dfa56b526 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.h @@ -65,7 +65,7 @@ class ConjugateGradientsSolver : public LinearSolver { private: const LinearSolver::Options options_; - DISALLOW_COPY_AND_ASSIGN(ConjugateGradientsSolver); + CERES_DISALLOW_COPY_AND_ASSIGN(ConjugateGradientsSolver); }; } // namespace internal diff --git a/extern/libmv/third_party/ceres/internal/ceres/corrector.cc b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc index 4ca2c6f6c86..eff4dff8566 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/corrector.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc @@ -32,8 +32,8 @@ #include <cstddef> #include <cmath> -#include <glog/logging.h> #include "ceres/internal/eigen.h" +#include "glog/logging.h" namespace ceres { namespace internal { diff --git a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.cc b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.cc new file mode 100644 index 00000000000..ca36ce07614 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.cc @@ -0,0 +1,130 @@ +// 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: strandmark@google.com (Petter Strandmark) + +#ifndef CERES_NO_CXSPARSE + +#include "ceres/cxsparse.h" + +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +CXSparse::CXSparse() : scratch_size_(0), scratch_(NULL) { +} + +CXSparse::~CXSparse() { + if (scratch_size_ > 0) { + cs_free(scratch_); + } +} + +bool CXSparse::SolveCholesky(cs_di* A, + cs_dis* symbolic_factorization, + double* b) { + // Make sure we have enough scratch space available. + if (scratch_size_ < A->n) { + if (scratch_size_ > 0) { + cs_free(scratch_); + } + scratch_ = reinterpret_cast<CS_ENTRY*>(cs_malloc(A->n, sizeof(CS_ENTRY))); + } + + // Solve using Cholesky factorization + csn* numeric_factorization = cs_chol(A, symbolic_factorization); + if (numeric_factorization == NULL) { + LOG(WARNING) << "Cholesky factorization failed."; + return false; + } + + // When the Cholesky factorization succeeded, these methods are guaranteed to + // succeeded as well. In the comments below, "x" refers to the scratch space. + // + // Set x = P * b. + cs_ipvec(symbolic_factorization->pinv, b, scratch_, A->n); + + // Set x = L \ x. + cs_lsolve(numeric_factorization->L, scratch_); + + // Set x = L' \ x. + cs_ltsolve(numeric_factorization->L, scratch_); + + // Set b = P' * x. + cs_pvec(symbolic_factorization->pinv, scratch_, b, A->n); + + // Free Cholesky factorization. + cs_nfree(numeric_factorization); + return true; +} + +cs_dis* CXSparse::AnalyzeCholesky(cs_di* A) { + // order = 1 for Cholesky factorization. + return cs_schol(1, A); +} + +cs_di CXSparse::CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A) { + cs_di At; + At.m = A->num_cols(); + At.n = A->num_rows(); + At.nz = -1; + At.nzmax = A->num_nonzeros(); + At.p = A->mutable_rows(); + At.i = A->mutable_cols(); + At.x = A->mutable_values(); + return At; +} + +cs_di* CXSparse::CreateSparseMatrix(TripletSparseMatrix* tsm) { + cs_di_sparse tsm_wrapper; + tsm_wrapper.nzmax = tsm->num_nonzeros();; + tsm_wrapper.nz = tsm->num_nonzeros();; + tsm_wrapper.m = tsm->num_rows(); + tsm_wrapper.n = tsm->num_cols(); + tsm_wrapper.p = tsm->mutable_cols(); + tsm_wrapper.i = tsm->mutable_rows(); + tsm_wrapper.x = tsm->mutable_values(); + + return cs_compress(&tsm_wrapper); +} + +void CXSparse::Free(cs_di* factor) { + cs_free(factor); +} + +void CXSparse::Free(cs_dis* factor) { + cs_sfree(factor); +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_CXSPARSE diff --git a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h new file mode 100644 index 00000000000..d3b64fcd1a5 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h @@ -0,0 +1,90 @@ +// 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: strandmark@google.com (Petter Strandmark) + +#ifndef CERES_INTERNAL_CXSPARSE_H_ +#define CERES_INTERNAL_CXSPARSE_H_ + +#ifndef CERES_NO_CXSPARSE + +#include "cs.h" + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; +class TripletSparseMatrix; + +// This object provides access to solving linear systems using Cholesky +// factorization with a known symbolic factorization. This features does not +// explicity exist in CXSparse. The methods in the class are nonstatic because +// the class manages internal scratch space. +class CXSparse { + public: + CXSparse(); + ~CXSparse(); + + // Solves a symmetric linear system A * x = b using Cholesky factorization. + // A - The system matrix. + // symbolic_factorization - The symbolic factorization of A. This is obtained + // from AnalyzeCholesky. + // b - The right hand size of the linear equation. This + // array will also recieve the solution. + // Returns false if Cholesky factorization of A fails. + bool SolveCholesky(cs_di* A, cs_dis* symbolic_factorization, double* b); + + // Creates a sparse matrix from a compressed-column form. No memory is + // allocated or copied; the structure A is filled out with info from the + // argument. + cs_di CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A); + + // Creates a new matrix from a triplet form. Deallocate the returned matrix + // with Free. May return NULL if the compression or allocation fails. + cs_di* CreateSparseMatrix(TripletSparseMatrix* A); + + // Computes a symbolic factorization of A that can be used in SolveCholesky. + // The returned matrix should be deallocated with Free when not used anymore. + cs_dis* AnalyzeCholesky(cs_di* A); + + // Deallocates the memory of a matrix obtained from AnalyzeCholesky. + void Free(cs_di* factor); + void Free(cs_dis* factor); + + private: + // Cached scratch space + CS_ENTRY* scratch_; + int scratch_size_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_CXSPARSE + +#endif // CERES_INTERNAL_CXSPARSE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.cc new file mode 100644 index 00000000000..f6bb99abf63 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.cc @@ -0,0 +1,86 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/dense_normal_cholesky_solver.h" + +#include <cstddef> + +#include "Eigen/Dense" +#include "ceres/dense_sparse_matrix.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 { + +DenseNormalCholeskySolver::DenseNormalCholeskySolver( + const LinearSolver::Options& options) + : options_(options) {} + +LinearSolver::Summary DenseNormalCholeskySolver::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(); + + ConstAlignedMatrixRef Aref = A->matrix(); + Matrix lhs(num_cols, num_cols); + lhs.setZero(); + + // lhs += A'A + // + // Using rankUpdate instead of GEMM, exposes the fact that its the + // same matrix being multiplied with itself and that the product is + // symmetric. + lhs.selfadjointView<Eigen::Upper>().rankUpdate(Aref.transpose()); + + // rhs = A'b + Vector rhs = Aref.transpose() * ConstVectorRef(b, num_rows); + + if (per_solve_options.D != NULL) { + ConstVectorRef D(per_solve_options.D, num_cols); + lhs += D.array().square().matrix().asDiagonal(); + } + + VectorRef(x, num_cols) = + lhs.selfadjointView<Eigen::Upper>().ldlt().solve(rhs); + + 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_normal_cholesky_solver.h b/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.h new file mode 100644 index 00000000000..de47740583d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.h @@ -0,0 +1,95 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) +// +// Solve dense rectangular systems Ax = b by forming the normal +// equations and solving them using the Cholesky factorization. + +#ifndef CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_SOLVER_H_ +#define CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_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 +// +// Setting the gradient of the above optimization problem to zero +// gives us the normal equations +// +// A'Ax = A'b +// +// A'A is a positive definite matrix (hopefully), and the resulting +// linear system can be solved using Cholesky factorization. +// +// 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 LDLT 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 DenseNormalCholeskySolver: public DenseSparseMatrixSolver { + public: + explicit DenseNormalCholeskySolver(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_; + CERES_DISALLOW_COPY_AND_ASSIGN(DenseNormalCholeskySolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_SOLVER_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 index 328505404d7..2b329ee0e9c 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc @@ -33,8 +33,8 @@ #include <cstddef> #include "Eigen/Dense" +#include "ceres/dense_sparse_matrix.h" #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" @@ -62,7 +62,7 @@ LinearSolver::Summary DenseQRSolver::SolveImpl( } // rhs = [b;0] to account for the additional rows in the lhs. - Vector rhs(num_rows + ((per_solve_options.D !=NULL) ? num_cols : 0)); + Vector rhs(num_rows + ((per_solve_options.D != NULL) ? num_cols : 0)); rhs.setZero(); rhs.head(num_rows) = ConstVectorRef(b, num_rows); 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 index 990c8d445eb..dd683a8c4ea 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h @@ -28,7 +28,7 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) // -// Solve dense rectangular systems Ax = b using the QR factoriztion. +// Solve dense rectangular systems Ax = b using the QR factorization. #ifndef CERES_INTERNAL_DENSE_QR_SOLVER_H_ #define CERES_INTERNAL_DENSE_QR_SOLVER_H_ @@ -90,7 +90,7 @@ class DenseQRSolver: public DenseSparseMatrixSolver { double* x); const LinearSolver::Options options_; - DISALLOW_COPY_AND_ASSIGN(DenseQRSolver); + CERES_DISALLOW_COPY_AND_ASSIGN(DenseQRSolver); }; } // namespace internal 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 index 5d392ba6c3b..86730cc101b 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc @@ -67,7 +67,7 @@ DenseSparseMatrix::DenseSparseMatrix(const Matrix& m) has_diagonal_reserved_(false) { } -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS DenseSparseMatrix::DenseSparseMatrix(const SparseMatrixProto& outer_proto) : m_(Eigen::MatrixXd::Zero( outer_proto.dense_matrix().num_rows(), @@ -108,7 +108,7 @@ void DenseSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { *dense_matrix = m_; } -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS void DenseSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { CHECK(!has_diagonal_appended_) << "Not supported."; outer_proto->Clear(); @@ -183,7 +183,7 @@ 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() - m_.cols()) : m_.rows(); for (int r = 0; r < active_rows; ++r) { 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 index 416c2143c2c..e7ad14d0ee6 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h @@ -52,7 +52,7 @@ class DenseSparseMatrix : public SparseMatrix { // 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 +#ifndef CERES_NO_PROTOCOL_BUFFERS explicit DenseSparseMatrix(const SparseMatrixProto& proto); #endif @@ -67,7 +67,7 @@ class DenseSparseMatrix : public SparseMatrix { 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 +#ifndef CERES_NO_PROTOCOL_BUFFERS virtual void ToProto(SparseMatrixProto* proto) const; #endif virtual void ToTextFile(FILE* file) const; diff --git a/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc index e9755043bab..ea5bf2e9690 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc @@ -28,9 +28,9 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) -#include <glog/logging.h> #include "ceres/detect_structure.h" #include "ceres/internal/eigen.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -60,10 +60,10 @@ void DetectStructure(const CompressedRowBlockStructure& bs, *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; + *row_block_size = Eigen::Dynamic; } @@ -71,10 +71,10 @@ void DetectStructure(const CompressedRowBlockStructure& bs, *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; + *e_block_size = Eigen::Dynamic; } if (*f_block_size == 0) { @@ -85,11 +85,11 @@ void DetectStructure(const CompressedRowBlockStructure& bs, } 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; + << "changed from " << *f_block_size << " to " + << bs.cols[row.cells[c].block_id].size; + *f_block_size = Eigen::Dynamic; + break; } } } diff --git a/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h index 8af4f236690..5f8e1b4ff46 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h +++ b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h @@ -49,7 +49,7 @@ namespace internal { // 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 +// schur_eliminator.h. This information is used to initialized an // appropriate template specialization of SchurEliminator. void DetectStructure(const CompressedRowBlockStructure& bs, const int num_eliminate_blocks, diff --git a/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.cc b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.cc new file mode 100644 index 00000000000..668fa54b8b8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.cc @@ -0,0 +1,691 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/dogleg_strategy.h" + +#include <cmath> +#include "Eigen/Dense" +#include "ceres/array_utils.h" +#include "ceres/internal/eigen.h" +#include "ceres/linear_solver.h" +#include "ceres/polynomial_solver.h" +#include "ceres/sparse_matrix.h" +#include "ceres/trust_region_strategy.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { +const double kMaxMu = 1.0; +const double kMinMu = 1e-8; +} + +DoglegStrategy::DoglegStrategy(const TrustRegionStrategy::Options& options) + : linear_solver_(options.linear_solver), + radius_(options.initial_radius), + max_radius_(options.max_radius), + min_diagonal_(options.lm_min_diagonal), + max_diagonal_(options.lm_max_diagonal), + mu_(kMinMu), + min_mu_(kMinMu), + max_mu_(kMaxMu), + mu_increase_factor_(10.0), + increase_threshold_(0.75), + decrease_threshold_(0.25), + dogleg_step_norm_(0.0), + reuse_(false), + dogleg_type_(options.dogleg_type) { + CHECK_NOTNULL(linear_solver_); + CHECK_GT(min_diagonal_, 0.0); + CHECK_LE(min_diagonal_, max_diagonal_); + CHECK_GT(max_radius_, 0.0); +} + +// If the reuse_ flag is not set, then the Cauchy point (scaled +// gradient) and the new Gauss-Newton step are computed from +// scratch. The Dogleg step is then computed as interpolation of these +// two vectors. +TrustRegionStrategy::Summary DoglegStrategy::ComputeStep( + const TrustRegionStrategy::PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals, + double* step) { + CHECK_NOTNULL(jacobian); + CHECK_NOTNULL(residuals); + CHECK_NOTNULL(step); + + const int n = jacobian->num_cols(); + if (reuse_) { + // Gauss-Newton and gradient vectors are always available, only a + // new interpolant need to be computed. For the subspace case, + // the subspace and the two-dimensional model are also still valid. + switch(dogleg_type_) { + case TRADITIONAL_DOGLEG: + ComputeTraditionalDoglegStep(step); + break; + + case SUBSPACE_DOGLEG: + ComputeSubspaceDoglegStep(step); + break; + } + TrustRegionStrategy::Summary summary; + summary.num_iterations = 0; + summary.termination_type = TOLERANCE; + return summary; + } + + reuse_ = true; + // Check that we have the storage needed to hold the various + // temporary vectors. + if (diagonal_.rows() != n) { + diagonal_.resize(n, 1); + gradient_.resize(n, 1); + gauss_newton_step_.resize(n, 1); + } + + // Vector used to form the diagonal matrix that is used to + // regularize the Gauss-Newton solve and that defines the + // elliptical trust region + // + // || D * step || <= radius_ . + // + jacobian->SquaredColumnNorm(diagonal_.data()); + for (int i = 0; i < n; ++i) { + diagonal_[i] = min(max(diagonal_[i], min_diagonal_), max_diagonal_); + } + diagonal_ = diagonal_.array().sqrt(); + + ComputeGradient(jacobian, residuals); + ComputeCauchyPoint(jacobian); + + LinearSolver::Summary linear_solver_summary = + ComputeGaussNewtonStep(jacobian, residuals); + + TrustRegionStrategy::Summary summary; + summary.residual_norm = linear_solver_summary.residual_norm; + summary.num_iterations = linear_solver_summary.num_iterations; + summary.termination_type = linear_solver_summary.termination_type; + + if (linear_solver_summary.termination_type != FAILURE) { + switch(dogleg_type_) { + // Interpolate the Cauchy point and the Gauss-Newton step. + case TRADITIONAL_DOGLEG: + ComputeTraditionalDoglegStep(step); + break; + + // Find the minimum in the subspace defined by the + // Cauchy point and the (Gauss-)Newton step. + case SUBSPACE_DOGLEG: + if (!ComputeSubspaceModel(jacobian)) { + summary.termination_type = FAILURE; + break; + } + ComputeSubspaceDoglegStep(step); + break; + } + } + + return summary; +} + +// The trust region is assumed to be elliptical with the +// diagonal scaling matrix D defined by sqrt(diagonal_). +// It is implemented by substituting step' = D * step. +// The trust region for step' is spherical. +// The gradient, the Gauss-Newton step, the Cauchy point, +// and all calculations involving the Jacobian have to +// be adjusted accordingly. +void DoglegStrategy::ComputeGradient( + SparseMatrix* jacobian, + const double* residuals) { + gradient_.setZero(); + jacobian->LeftMultiply(residuals, gradient_.data()); + gradient_.array() /= diagonal_.array(); +} + +// The Cauchy point is the global minimizer of the quadratic model +// along the one-dimensional subspace spanned by the gradient. +void DoglegStrategy::ComputeCauchyPoint(SparseMatrix* jacobian) { + // alpha * -gradient is the Cauchy point. + Vector Jg(jacobian->num_rows()); + Jg.setZero(); + // The Jacobian is scaled implicitly by computing J * (D^-1 * (D^-1 * g)) + // instead of (J * D^-1) * (D^-1 * g). + Vector scaled_gradient = + (gradient_.array() / diagonal_.array()).matrix(); + jacobian->RightMultiply(scaled_gradient.data(), Jg.data()); + alpha_ = gradient_.squaredNorm() / Jg.squaredNorm(); +} + +// The dogleg step is defined as the intersection of the trust region +// boundary with the piecewise linear path from the origin to the Cauchy +// point and then from there to the Gauss-Newton point (global minimizer +// of the model function). The Gauss-Newton point is taken if it lies +// within the trust region. +void DoglegStrategy::ComputeTraditionalDoglegStep(double* dogleg) { + VectorRef dogleg_step(dogleg, gradient_.rows()); + + // Case 1. The Gauss-Newton step lies inside the trust region, and + // is therefore the optimal solution to the trust-region problem. + const double gradient_norm = gradient_.norm(); + const double gauss_newton_norm = gauss_newton_step_.norm(); + if (gauss_newton_norm <= radius_) { + dogleg_step = gauss_newton_step_; + dogleg_step_norm_ = gauss_newton_norm; + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "GaussNewton step size: " << dogleg_step_norm_ + << " radius: " << radius_; + return; + } + + // Case 2. The Cauchy point and the Gauss-Newton steps lie outside + // the trust region. Rescale the Cauchy point to the trust region + // and return. + if (gradient_norm * alpha_ >= radius_) { + dogleg_step = -(radius_ / gradient_norm) * gradient_; + dogleg_step_norm_ = radius_; + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "Cauchy step size: " << dogleg_step_norm_ + << " radius: " << radius_; + return; + } + + // Case 3. The Cauchy point is inside the trust region and the + // Gauss-Newton step is outside. Compute the line joining the two + // points and the point on it which intersects the trust region + // boundary. + + // a = alpha * -gradient + // b = gauss_newton_step + const double b_dot_a = -alpha_ * gradient_.dot(gauss_newton_step_); + const double a_squared_norm = pow(alpha_ * gradient_norm, 2.0); + const double b_minus_a_squared_norm = + a_squared_norm - 2 * b_dot_a + pow(gauss_newton_norm, 2); + + // c = a' (b - a) + // = alpha * -gradient' gauss_newton_step - alpha^2 |gradient|^2 + const double c = b_dot_a - a_squared_norm; + const double d = sqrt(c * c + b_minus_a_squared_norm * + (pow(radius_, 2.0) - a_squared_norm)); + + double beta = + (c <= 0) + ? (d - c) / b_minus_a_squared_norm + : (radius_ * radius_ - a_squared_norm) / (d + c); + dogleg_step = (-alpha_ * (1.0 - beta)) * gradient_ + + beta * gauss_newton_step_; + dogleg_step_norm_ = dogleg_step.norm(); + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "Dogleg step size: " << dogleg_step_norm_ + << " radius: " << radius_; +} + +// The subspace method finds the minimum of the two-dimensional problem +// +// min. 1/2 x' B' H B x + g' B x +// s.t. || B x ||^2 <= r^2 +// +// where r is the trust region radius and B is the matrix with unit columns +// spanning the subspace defined by the steepest descent and Newton direction. +// This subspace by definition includes the Gauss-Newton point, which is +// therefore taken if it lies within the trust region. +void DoglegStrategy::ComputeSubspaceDoglegStep(double* dogleg) { + VectorRef dogleg_step(dogleg, gradient_.rows()); + + // The Gauss-Newton point is inside the trust region if |GN| <= radius_. + // This test is valid even though radius_ is a length in the two-dimensional + // subspace while gauss_newton_step_ is expressed in the (scaled) + // higher dimensional original space. This is because + // + // 1. gauss_newton_step_ by definition lies in the subspace, and + // 2. the subspace basis is orthonormal. + // + // As a consequence, the norm of the gauss_newton_step_ in the subspace is + // the same as its norm in the original space. + const double gauss_newton_norm = gauss_newton_step_.norm(); + if (gauss_newton_norm <= radius_) { + dogleg_step = gauss_newton_step_; + dogleg_step_norm_ = gauss_newton_norm; + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "GaussNewton step size: " << dogleg_step_norm_ + << " radius: " << radius_; + return; + } + + // The optimum lies on the boundary of the trust region. The above problem + // therefore becomes + // + // min. 1/2 x^T B^T H B x + g^T B x + // s.t. || B x ||^2 = r^2 + // + // Notice the equality in the constraint. + // + // This can be solved by forming the Lagrangian, solving for x(y), where + // y is the Lagrange multiplier, using the gradient of the objective, and + // putting x(y) back into the constraint. This results in a fourth order + // polynomial in y, which can be solved using e.g. the companion matrix. + // See the description of MakePolynomialForBoundaryConstrainedProblem for + // details. The result is up to four real roots y*, not all of which + // correspond to feasible points. The feasible points x(y*) have to be + // tested for optimality. + + if (subspace_is_one_dimensional_) { + // The subspace is one-dimensional, so both the gradient and + // the Gauss-Newton step point towards the same direction. + // In this case, we move along the gradient until we reach the trust + // region boundary. + dogleg_step = -(radius_ / gradient_.norm()) * gradient_; + dogleg_step_norm_ = radius_; + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "Dogleg subspace step size (1D): " << dogleg_step_norm_ + << " radius: " << radius_; + return; + } + + Vector2d minimum(0.0, 0.0); + if (!FindMinimumOnTrustRegionBoundary(&minimum)) { + // For the positive semi-definite case, a traditional dogleg step + // is taken in this case. + LOG(WARNING) << "Failed to compute polynomial roots. " + << "Taking traditional dogleg step instead."; + ComputeTraditionalDoglegStep(dogleg); + return; + } + + // Test first order optimality at the minimum. + // The first order KKT conditions state that the minimum x* + // has to satisfy either || x* ||^2 < r^2 (i.e. has to lie within + // the trust region), or + // + // (B x* + g) + y x* = 0 + // + // for some positive scalar y. + // Here, as it is already known that the minimum lies on the boundary, the + // latter condition is tested. To allow for small imprecisions, we test if + // the angle between (B x* + g) and -x* is smaller than acos(0.99). + // The exact value of the cosine is arbitrary but should be close to 1. + // + // This condition should not be violated. If it is, the minimum was not + // correctly determined. + const double kCosineThreshold = 0.99; + const Vector2d grad_minimum = subspace_B_ * minimum + subspace_g_; + const double cosine_angle = -minimum.dot(grad_minimum) / + (minimum.norm() * grad_minimum.norm()); + if (cosine_angle < kCosineThreshold) { + LOG(WARNING) << "First order optimality seems to be violated " + << "in the subspace method!\n" + << "Cosine of angle between x and B x + g is " + << cosine_angle << ".\n" + << "Taking a regular dogleg step instead.\n" + << "Please consider filing a bug report if this " + << "happens frequently or consistently.\n"; + ComputeTraditionalDoglegStep(dogleg); + return; + } + + // Create the full step from the optimal 2d solution. + dogleg_step = subspace_basis_ * minimum; + dogleg_step_norm_ = radius_; + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "Dogleg subspace step size: " << dogleg_step_norm_ + << " radius: " << radius_; +} + +// Build the polynomial that defines the optimal Lagrange multipliers. +// Let the Lagrangian be +// +// L(x, y) = 0.5 x^T B x + x^T g + y (0.5 x^T x - 0.5 r^2). (1) +// +// Stationary points of the Lagrangian are given by +// +// 0 = d L(x, y) / dx = Bx + g + y x (2) +// 0 = d L(x, y) / dy = 0.5 x^T x - 0.5 r^2 (3) +// +// For any given y, we can solve (2) for x as +// +// x(y) = -(B + y I)^-1 g . (4) +// +// As B + y I is 2x2, we form the inverse explicitly: +// +// (B + y I)^-1 = (1 / det(B + y I)) adj(B + y I) (5) +// +// where adj() denotes adjugation. This should be safe, as B is positive +// semi-definite and y is necessarily positive, so (B + y I) is indeed +// invertible. +// Plugging (5) into (4) and the result into (3), then dividing by 0.5 we +// obtain +// +// 0 = (1 / det(B + y I))^2 g^T adj(B + y I)^T adj(B + y I) g - r^2 +// (6) +// +// or +// +// det(B + y I)^2 r^2 = g^T adj(B + y I)^T adj(B + y I) g (7a) +// = g^T adj(B)^T adj(B) g +// + 2 y g^T adj(B)^T g + y^2 g^T g (7b) +// +// as +// +// adj(B + y I) = adj(B) + y I = adj(B)^T + y I . (8) +// +// The left hand side can be expressed explicitly using +// +// det(B + y I) = det(B) + y tr(B) + y^2 . (9) +// +// So (7) is a polynomial in y of degree four. +// Bringing everything back to the left hand side, the coefficients can +// be read off as +// +// y^4 r^2 +// + y^3 2 r^2 tr(B) +// + y^2 (r^2 tr(B)^2 + 2 r^2 det(B) - g^T g) +// + y^1 (2 r^2 det(B) tr(B) - 2 g^T adj(B)^T g) +// + y^0 (r^2 det(B)^2 - g^T adj(B)^T adj(B) g) +// +Vector DoglegStrategy::MakePolynomialForBoundaryConstrainedProblem() const { + const double detB = subspace_B_.determinant(); + const double trB = subspace_B_.trace(); + const double r2 = radius_ * radius_; + Matrix2d B_adj; + B_adj << subspace_B_(1,1) , -subspace_B_(0,1), + -subspace_B_(1,0) , subspace_B_(0,0); + + Vector polynomial(5); + polynomial(0) = r2; + polynomial(1) = 2.0 * r2 * trB; + polynomial(2) = r2 * ( trB * trB + 2.0 * detB ) - subspace_g_.squaredNorm(); + polynomial(3) = -2.0 * ( subspace_g_.transpose() * B_adj * subspace_g_ + - r2 * detB * trB ); + polynomial(4) = r2 * detB * detB - (B_adj * subspace_g_).squaredNorm(); + + return polynomial; +} + +// Given a Lagrange multiplier y that corresponds to a stationary point +// of the Lagrangian L(x, y), compute the corresponding x from the +// equation +// +// 0 = d L(x, y) / dx +// = B * x + g + y * x +// = (B + y * I) * x + g +// +DoglegStrategy::Vector2d DoglegStrategy::ComputeSubspaceStepFromRoot( + double y) const { + const Matrix2d B_i = subspace_B_ + y * Matrix2d::Identity(); + return -B_i.partialPivLu().solve(subspace_g_); +} + +// This function evaluates the quadratic model at a point x in the +// subspace spanned by subspace_basis_. +double DoglegStrategy::EvaluateSubspaceModel(const Vector2d& x) const { + return 0.5 * x.dot(subspace_B_ * x) + subspace_g_.dot(x); +} + +// This function attempts to solve the boundary-constrained subspace problem +// +// min. 1/2 x^T B^T H B x + g^T B x +// s.t. || B x ||^2 = r^2 +// +// where B is an orthonormal subspace basis and r is the trust-region radius. +// +// This is done by finding the roots of a fourth degree polynomial. If the +// root finding fails, the function returns false and minimum will be set +// to (0, 0). If it succeeds, true is returned. +// +// In the failure case, another step should be taken, such as the traditional +// dogleg step. +bool DoglegStrategy::FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const { + CHECK_NOTNULL(minimum); + + // Return (0, 0) in all error cases. + minimum->setZero(); + + // Create the fourth-degree polynomial that is a necessary condition for + // optimality. + const Vector polynomial = MakePolynomialForBoundaryConstrainedProblem(); + + // Find the real parts y_i of its roots (not only the real roots). + Vector roots_real; + if (!FindPolynomialRoots(polynomial, &roots_real, NULL)) { + // Failed to find the roots of the polynomial, i.e. the candidate + // solutions of the constrained problem. Report this back to the caller. + return false; + } + + // For each root y, compute B x(y) and check for feasibility. + // Notice that there should always be four roots, as the leading term of + // the polynomial is r^2 and therefore non-zero. However, as some roots + // may be complex, the real parts are not necessarily unique. + double minimum_value = std::numeric_limits<double>::max(); + bool valid_root_found = false; + for (int i = 0; i < roots_real.size(); ++i) { + const Vector2d x_i = ComputeSubspaceStepFromRoot(roots_real(i)); + + // Not all roots correspond to points on the trust region boundary. + // There are at most four candidate solutions. As we are interested + // in the minimum, it is safe to consider all of them after projecting + // them onto the trust region boundary. + if (x_i.norm() > 0) { + const double f_i = EvaluateSubspaceModel((radius_ / x_i.norm()) * x_i); + valid_root_found = true; + if (f_i < minimum_value) { + minimum_value = f_i; + *minimum = x_i; + } + } + } + + return valid_root_found; +} + +LinearSolver::Summary DoglegStrategy::ComputeGaussNewtonStep( + SparseMatrix* jacobian, + const double* residuals) { + const int n = jacobian->num_cols(); + LinearSolver::Summary linear_solver_summary; + linear_solver_summary.termination_type = FAILURE; + + // The Jacobian matrix is often quite poorly conditioned. Thus it is + // necessary to add a diagonal matrix at the bottom to prevent the + // linear solver from failing. + // + // We do this by computing the same diagonal matrix as the one used + // by Levenberg-Marquardt (other choices are possible), and scaling + // it by a small constant (independent of the trust region radius). + // + // If the solve fails, the multiplier to the diagonal is increased + // up to max_mu_ by a factor of mu_increase_factor_ every time. If + // the linear solver is still not successful, the strategy returns + // with FAILURE. + // + // Next time when a new Gauss-Newton step is requested, the + // multiplier starts out from the last successful solve. + // + // When a step is declared successful, the multiplier is decreased + // by half of mu_increase_factor_. + + while (mu_ < max_mu_) { + // Dogleg, as far as I (sameeragarwal) understand it, requires a + // reasonably good estimate of the Gauss-Newton step. This means + // that we need to solve the normal equations more or less + // exactly. This is reflected in the values of the tolerances set + // below. + // + // For now, this strategy should only be used with exact + // factorization based solvers, for which these tolerances are + // automatically satisfied. + // + // The right way to combine inexact solves with trust region + // methods is to use Stiehaug's method. + LinearSolver::PerSolveOptions solve_options; + solve_options.q_tolerance = 0.0; + solve_options.r_tolerance = 0.0; + + lm_diagonal_ = diagonal_ * std::sqrt(mu_); + solve_options.D = lm_diagonal_.data(); + + // As in the LevenbergMarquardtStrategy, solve Jy = r instead + // of Jx = -r and later set x = -y to avoid having to modify + // either jacobian or residuals. + InvalidateArray(n, gauss_newton_step_.data()); + linear_solver_summary = linear_solver_->Solve(jacobian, + residuals, + solve_options, + gauss_newton_step_.data()); + + if (linear_solver_summary.termination_type == FAILURE || + !IsArrayValid(n, gauss_newton_step_.data())) { + mu_ *= mu_increase_factor_; + VLOG(2) << "Increasing mu " << mu_; + linear_solver_summary.termination_type = FAILURE; + continue; + } + break; + } + + if (linear_solver_summary.termination_type != FAILURE) { + // The scaled Gauss-Newton step is D * GN: + // + // - (D^-1 J^T J D^-1)^-1 (D^-1 g) + // = - D (J^T J)^-1 D D^-1 g + // = D -(J^T J)^-1 g + // + gauss_newton_step_.array() *= -diagonal_.array(); + } + + return linear_solver_summary; +} + +void DoglegStrategy::StepAccepted(double step_quality) { + CHECK_GT(step_quality, 0.0); + + if (step_quality < decrease_threshold_) { + radius_ *= 0.5; + } + + if (step_quality > increase_threshold_) { + radius_ = max(radius_, 3.0 * dogleg_step_norm_); + } + + // Reduce the regularization multiplier, in the hope that whatever + // was causing the rank deficiency has gone away and we can return + // to doing a pure Gauss-Newton solve. + mu_ = max(min_mu_, 2.0 * mu_ / mu_increase_factor_ ); + reuse_ = false; +} + +void DoglegStrategy::StepRejected(double step_quality) { + radius_ *= 0.5; + reuse_ = true; +} + +void DoglegStrategy::StepIsInvalid() { + mu_ *= mu_increase_factor_; + reuse_ = false; +} + +double DoglegStrategy::Radius() const { + return radius_; +} + +bool DoglegStrategy::ComputeSubspaceModel(SparseMatrix* jacobian) { + // Compute an orthogonal basis for the subspace using QR decomposition. + Matrix basis_vectors(jacobian->num_cols(), 2); + basis_vectors.col(0) = gradient_; + basis_vectors.col(1) = gauss_newton_step_; + Eigen::ColPivHouseholderQR<Matrix> basis_qr(basis_vectors); + + switch (basis_qr.rank()) { + case 0: + // This should never happen, as it implies that both the gradient + // and the Gauss-Newton step are zero. In this case, the minimizer should + // have stopped due to the gradient being too small. + LOG(ERROR) << "Rank of subspace basis is 0. " + << "This means that the gradient at the current iterate is " + << "zero but the optimization has not been terminated. " + << "You may have found a bug in Ceres."; + return false; + + case 1: + // Gradient and Gauss-Newton step coincide, so we lie on one of the + // major axes of the quadratic problem. In this case, we simply move + // along the gradient until we reach the trust region boundary. + subspace_is_one_dimensional_ = true; + return true; + + case 2: + subspace_is_one_dimensional_ = false; + break; + + default: + LOG(ERROR) << "Rank of the subspace basis matrix is reported to be " + << "greater than 2. As the matrix contains only two " + << "columns this cannot be true and is indicative of " + << "a bug."; + return false; + } + + // The subspace is two-dimensional, so compute the subspace model. + // Given the basis U, this is + // + // subspace_g_ = g_scaled^T U + // + // and + // + // subspace_B_ = U^T (J_scaled^T J_scaled) U + // + // As J_scaled = J * D^-1, the latter becomes + // + // subspace_B_ = ((U^T D^-1) J^T) (J (D^-1 U)) + // = (J (D^-1 U))^T (J (D^-1 U)) + + subspace_basis_ = + basis_qr.householderQ() * Matrix::Identity(jacobian->num_cols(), 2); + + subspace_g_ = subspace_basis_.transpose() * gradient_; + + Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::RowMajor> + Jb(2, jacobian->num_rows()); + Jb.setZero(); + + Vector tmp; + tmp = (subspace_basis_.col(0).array() / diagonal_.array()).matrix(); + jacobian->RightMultiply(tmp.data(), Jb.row(0).data()); + tmp = (subspace_basis_.col(1).array() / diagonal_.array()).matrix(); + jacobian->RightMultiply(tmp.data(), Jb.row(1).data()); + + subspace_B_ = Jb * Jb.transpose(); + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.h b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.h new file mode 100644 index 00000000000..bff1689aa4a --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.h @@ -0,0 +1,163 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_DOGLEG_STRATEGY_H_ +#define CERES_INTERNAL_DOGLEG_STRATEGY_H_ + +#include "ceres/linear_solver.h" +#include "ceres/trust_region_strategy.h" + +namespace ceres { +namespace internal { + +// Dogleg step computation and trust region sizing strategy based on +// 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 +// +// One minor modification is that instead of computing the pure +// Gauss-Newton step, we compute a regularized version of it. This is +// because the Jacobian is often rank-deficient and in such cases +// using a direct solver leads to numerical failure. +// +// If SUBSPACE is passed as the type argument to the constructor, the +// DoglegStrategy follows the approach by Shultz, Schnabel, Byrd. +// This finds the exact optimum over the two-dimensional subspace +// spanned by the two Dogleg vectors. +class DoglegStrategy : public TrustRegionStrategy { +public: + DoglegStrategy(const TrustRegionStrategy::Options& options); + virtual ~DoglegStrategy() {} + + // TrustRegionStrategy interface + virtual Summary ComputeStep(const PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals, + double* step); + virtual void StepAccepted(double step_quality); + virtual void StepRejected(double step_quality); + virtual void StepIsInvalid(); + + virtual double Radius() const; + + // These functions are predominantly for testing. + Vector gradient() const { return gradient_; } + Vector gauss_newton_step() const { return gauss_newton_step_; } + Matrix subspace_basis() const { return subspace_basis_; } + Vector subspace_g() const { return subspace_g_; } + Matrix subspace_B() const { return subspace_B_; } + + private: + typedef Eigen::Matrix<double, 2, 1, Eigen::DontAlign> Vector2d; + typedef Eigen::Matrix<double, 2, 2, Eigen::DontAlign> Matrix2d; + + LinearSolver::Summary ComputeGaussNewtonStep(SparseMatrix* jacobian, + const double* residuals); + void ComputeCauchyPoint(SparseMatrix* jacobian); + void ComputeGradient(SparseMatrix* jacobian, const double* residuals); + void ComputeTraditionalDoglegStep(double* step); + bool ComputeSubspaceModel(SparseMatrix* jacobian); + void ComputeSubspaceDoglegStep(double* step); + + bool FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const; + Vector MakePolynomialForBoundaryConstrainedProblem() const; + Vector2d ComputeSubspaceStepFromRoot(double lambda) const; + double EvaluateSubspaceModel(const Vector2d& x) const; + + LinearSolver* linear_solver_; + double radius_; + const double max_radius_; + + const double min_diagonal_; + const double max_diagonal_; + + // mu is used to scale the diagonal matrix used to make the + // Gauss-Newton solve full rank. In each solve, the strategy starts + // out with mu = min_mu, and tries values upto max_mu. If the user + // reports an invalid step, the value of mu_ is increased so that + // the next solve starts with a stronger regularization. + // + // If a successful step is reported, then the value of mu_ is + // decreased with a lower bound of min_mu_. + double mu_; + const double min_mu_; + const double max_mu_; + const double mu_increase_factor_; + const double increase_threshold_; + const double decrease_threshold_; + + Vector diagonal_; // sqrt(diag(J^T J)) + Vector lm_diagonal_; + + Vector gradient_; + Vector gauss_newton_step_; + + // cauchy_step = alpha * gradient + double alpha_; + double dogleg_step_norm_; + + // When, ComputeStep is called, reuse_ indicates whether the + // Gauss-Newton and Cauchy steps from the last call to ComputeStep + // can be reused or not. + // + // If the user called StepAccepted, then it is expected that the + // user has recomputed the Jacobian matrix and new Gauss-Newton + // solve is needed and reuse is set to false. + // + // If the user called StepRejected, then it is expected that the + // user wants to solve the trust region problem with the same matrix + // but a different trust region radius and the Gauss-Newton and + // Cauchy steps can be reused to compute the Dogleg, thus reuse is + // set to true. + // + // If the user called StepIsInvalid, then there was a numerical + // problem with the step computed in the last call to ComputeStep, + // and the regularization used to do the Gauss-Newton solve is + // increased and a new solve should be done when ComputeStep is + // called again, thus reuse is set to false. + bool reuse_; + + // The dogleg type determines how the minimum of the local + // quadratic model is found. + DoglegType dogleg_type_; + + // If the type is SUBSPACE_DOGLEG, the two-dimensional + // model 1/2 x^T B x + g^T x has to be computed and stored. + bool subspace_is_one_dimensional_; + Matrix subspace_basis_; + Vector2d subspace_g_; + Matrix2d subspace_B_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DOGLEG_STRATEGY_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc b/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc index ea05aefec8c..a3ce6f04bd4 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc @@ -28,14 +28,18 @@ // // Author: keir@google.com (Keir Mierle) -#include <glog/logging.h> -#include "ceres/evaluator.h" +#include <vector> #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/compressed_row_sparse_matrix.h" +#include "ceres/crs_matrix.h" #include "ceres/dense_jacobian_writer.h" +#include "ceres/evaluator.h" +#include "ceres/internal/port.h" #include "ceres/program_evaluator.h" +#include "ceres/scratch_evaluate_preparer.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -47,6 +51,7 @@ Evaluator* Evaluator::Create(const Evaluator::Options& options, string* error) { switch (options.linear_solver_type) { case DENSE_QR: + case DENSE_NORMAL_CHOLESKY: return new ProgramEvaluator<ScratchEvaluatePreparer, DenseJacobianWriter>(options, program); @@ -67,5 +72,76 @@ Evaluator* Evaluator::Create(const Evaluator::Options& options, } } +bool Evaluator::Evaluate(Program* program, + int num_threads, + double* cost, + vector<double>* residuals, + vector<double>* gradient, + CRSMatrix* output_jacobian) { + CHECK_GE(num_threads, 1) + << "This is a Ceres bug; please contact the developers!"; + CHECK_NOTNULL(cost); + + // Setup the Parameter indices and offsets before an evaluator can + // be constructed and used. + program->SetParameterOffsetsAndIndex(); + + Evaluator::Options evaluator_options; + evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; + evaluator_options.num_threads = num_threads; + + string error; + scoped_ptr<Evaluator> evaluator( + Evaluator::Create(evaluator_options, program, &error)); + if (evaluator.get() == NULL) { + LOG(ERROR) << "Unable to create an Evaluator object. " + << "Error: " << error + << "This is a Ceres bug; please contact the developers!"; + return false; + } + + if (residuals !=NULL) { + residuals->resize(evaluator->NumResiduals()); + } + + if (gradient != NULL) { + gradient->resize(evaluator->NumEffectiveParameters()); + } + + scoped_ptr<CompressedRowSparseMatrix> jacobian; + if (output_jacobian != NULL) { + jacobian.reset( + down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian())); + } + + // Point the state pointers to the user state pointers. This is + // needed so that we can extract a parameter vector which is then + // passed to Evaluator::Evaluate. + program->SetParameterBlockStatePtrsToUserStatePtrs(); + + // Copy the value of the parameter blocks into a vector, since the + // Evaluate::Evaluate method needs its input as such. The previous + // call to SetParameterBlockStatePtrsToUserStatePtrs ensures that + // these values are the ones corresponding to the actual state of + // the parameter blocks, rather than the temporary state pointer + // used for evaluation. + Vector parameters(program->NumParameters()); + program->ParameterBlocksToStateVector(parameters.data()); + + if (!evaluator->Evaluate(parameters.data(), + cost, + residuals != NULL ? &(*residuals)[0] : NULL, + gradient != NULL ? &(*gradient)[0] : NULL, + jacobian.get())) { + return false; + } + + if (output_jacobian != NULL) { + jacobian->ToCRSMatrix(output_jacobian); + } + + return true; +} + } // 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 index adefdd26660..6aa30d7b739 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/evaluator.h +++ b/extern/libmv/third_party/ceres/internal/ceres/evaluator.h @@ -33,10 +33,14 @@ #define CERES_INTERNAL_EVALUATOR_H_ #include <string> +#include <vector> #include "ceres/internal/port.h" #include "ceres/types.h" namespace ceres { + +class CRSMatrix; + namespace internal { class Program; @@ -65,6 +69,32 @@ class Evaluator { Program* program, string* error); + + // This is used for computing the cost, residual and Jacobian for + // returning to the user. For actually solving the optimization + // problem, the optimization algorithm uses the ProgramEvaluator + // objects directly. + // + // The residual, gradients and jacobian pointers can be NULL, in + // which case they will not be evaluated. cost cannot be NULL. + // + // The parallelism of the evaluator is controlled by num_threads; it + // should be at least 1. + // + // Note: That this function does not take a parameter vector as + // input. The parameter blocks are evaluated on the values contained + // in the arrays pointed to by their user_state pointers. + // + // Also worth noting is that this function mutates program by + // calling Program::SetParameterOffsetsAndIndex() on it so that an + // evaluator object can be constructed. + static bool Evaluate(Program* program, + int num_threads, + double* cost, + vector<double>* residuals, + vector<double>* gradient, + CRSMatrix* jacobian); + // 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 @@ -95,6 +125,7 @@ class Evaluator { virtual bool Evaluate(const double* state, double* cost, double* residuals, + double* gradient, SparseMatrix* jacobian) = 0; // Make a change delta (of size NumEffectiveParameters()) to state (of size diff --git a/extern/libmv/third_party/ceres/internal/ceres/file.cc b/extern/libmv/third_party/ceres/internal/ceres/file.cc index 5fc9d220861..387f359b2ee 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/file.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/file.cc @@ -31,7 +31,7 @@ // Really simple file IO. #include <cstdio> -#include <glog/logging.h> +#include "glog/logging.h" namespace ceres { namespace internal { @@ -48,7 +48,7 @@ void WriteStringToFileOrDie(const string &data, const string &filename) { } void ReadFileToStringOrDie(const string &filename, string *data) { - FILE* file_descriptor = file_descriptor = fopen(filename.c_str(), "r"); + FILE* file_descriptor = fopen(filename.c_str(), "r"); if (!file_descriptor) { LOG(FATAL) << "Couldn't read file: " << filename; diff --git a/extern/libmv/third_party/ceres/internal/ceres/generate_eliminator_specialization.py b/extern/libmv/third_party/ceres/internal/ceres/generate_eliminator_specialization.py new file mode 100644 index 00000000000..af9873f94c0 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generate_eliminator_specialization.py @@ -0,0 +1,186 @@ +// 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. +// +# +# Copyright 2011 Google Inc. All Rights Reserved. +# Author: sameeragarwal@google.com (Sameer Agarwal) +# +# Script for explicitly generating template specialization of the +# SchurEliminator class. It is a rather large class +# and the number of explicit instantiations is also large. Explicitly +# generating these instantiations in separate .cc files breaks the +# compilation into separate compilation unit rather than one large cc +# file which takes 2+GB of RAM to compile. +# +# This script creates two sets of files. +# +# 1. schur_eliminator_x_x_x.cc +# where, the x indicates the template parameters and +# +# 2. schur_eliminator.cc +# +# that contains a factory function for instantiating these classes +# based on runtime parameters. +# +# The list of tuples, specializations indicates the set of +# specializations that is generated. + +# Set of template specializations to generate +SPECIALIZATIONS = [(2, 2, 2), + (2, 2, 3), + (2, 2, 4), + (2, 2, "Dynamic"), + (2, 3, 3), + (2, 3, 4), + (2, 3, 9), + (2, 3, "Dynamic"), + (2, 4, 3), + (2, 4, 4), + (2, 4, "Dynamic"), + (4, 4, 2), + (4, 4, 3), + (4, 4, 4), + (4, 4, "Dynamic"), + ("Dynamic", "Dynamic", "Dynamic")] + +SPECIALIZATION_FILE = """// Copyright 2011 Google Inc. All Rights Reserved. +// 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<%s, %s, %s>; + +} // namespace internal +} // namespace ceres + +""" + +FACTORY_FILE_HEADER = """// Copyright 2011 Google Inc. All Rights Reserved. +// 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 +""" + +FACTORY_CONDITIONAL = """ if ((options.row_block_size == %s) && + (options.e_block_size == %s) && + (options.f_block_size == %s)) { + return new SchurEliminator<%s, %s, %s>(options); + } +""" + +FACTORY_FOOTER = """ +#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 +""" + + +def SuffixForSize(size): + if size == "Dynamic": + return "d" + return str(size) + + +def SpecializationFilename(prefix, row_block_size, e_block_size, f_block_size): + return "_".join([prefix] + map(SuffixForSize, (row_block_size, + e_block_size, + f_block_size))) + + +def Specialize(): + """ + Generate specialization code and the conditionals to instantiate it. + """ + f = open("schur_eliminator.cc", "w") + f.write(FACTORY_FILE_HEADER) + + for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS: + output = SpecializationFilename("generated/schur_eliminator", + row_block_size, + e_block_size, + f_block_size) + ".cc" + fptr = open(output, "w") + fptr.write(SPECIALIZATION_FILE % (row_block_size, + e_block_size, + f_block_size)) + fptr.close() + + f.write(FACTORY_CONDITIONAL % (row_block_size, + e_block_size, + f_block_size, + row_block_size, + e_block_size, + f_block_size)) + f.write(FACTORY_FOOTER) + f.close() + + +if __name__ == "__main__": + Specialize() 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 index abba40824ef..7fb3ed7b3a8 100644 --- 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 @@ -36,18 +36,18 @@ #include <string> #include <vector> -#include <glog/logging.h> +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" #include "ceres/parameter_block.h" +#include "ceres/problem.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" +#include "glog/logging.h" namespace ceres { namespace internal { diff --git a/extern/libmv/third_party/ceres/internal/ceres/graph.h b/extern/libmv/third_party/ceres/internal/ceres/graph.h index fd7a224f0aa..2c0f6d28e54 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/graph.h +++ b/extern/libmv/third_party/ceres/internal/ceres/graph.h @@ -129,7 +129,7 @@ class Graph { HashMap<Vertex, HashSet<Vertex> > edges_; HashMap<pair<Vertex, Vertex>, double> edge_weights_; - DISALLOW_COPY_AND_ASSIGN(Graph); + CERES_DISALLOW_COPY_AND_ASSIGN(Graph); }; } // namespace internal 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 index bd908846362..4af030a8535 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc @@ -30,22 +30,20 @@ #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" +#include "glog/logging.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), @@ -62,7 +60,7 @@ void ImplicitSchurComplement::Init(const BlockSparseMatrixBase& A, const double* b) { // Since initialization is reasonably heavy, perhaps we can save on // constructing a new object everytime. - if ((A_ == NULL) || !constant_sparsity_) { + if (A_ == NULL) { A_.reset(new PartitionedMatrixView(A, num_eliminate_blocks_)); } @@ -71,7 +69,7 @@ void ImplicitSchurComplement::Init(const BlockSparseMatrixBase& A, // Initialize temporary storage and compute the block diagonals of // E'E and F'E. - if ((!constant_sparsity_) || (block_diagonal_EtE_inverse_ == NULL)) { + if (block_diagonal_EtE_inverse_ == NULL) { block_diagonal_EtE_inverse_.reset(A_->CreateBlockDiagonalEtE()); if (preconditioner_) { block_diagonal_FtF_inverse_.reset(A_->CreateBlockDiagonalFtF()); @@ -92,17 +90,10 @@ void ImplicitSchurComplement::Init(const BlockSparseMatrixBase& A, // 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()); - } + AddDiagonalAndInvert(D_, block_diagonal_EtE_inverse_.get()); + if (preconditioner_) { + AddDiagonalAndInvert((D_ == NULL) ? NULL : D_ + A_->num_cols_e(), + block_diagonal_FtF_inverse_.get()); } // Compute the RHS of the Schur complement system. 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 index 37a319f9c57..b9ebaa4628e 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h +++ b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h @@ -91,20 +91,13 @@ class ImplicitSchurComplement : public LinearOperator { // 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); + ImplicitSchurComplement(int num_eliminate_blocks, bool preconditioner); virtual ~ImplicitSchurComplement(); // Initialize the Schur complement for a linear least squares @@ -151,7 +144,6 @@ class ImplicitSchurComplement : public LinearOperator { void UpdateRhs(); int num_eliminate_blocks_; - bool constant_sparsity_; bool preconditioner_; scoped_ptr<PartitionedMatrixView> A_; 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 index 51303195317..679c41f2431 100644 --- 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 @@ -33,22 +33,18 @@ #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" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -69,10 +65,9 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( CHECK_NOTNULL(A->block_structure()); // Initialize a ImplicitSchurComplement object. - if ((schur_complement_ == NULL) || (!options_.constant_sparsity)) { + if (schur_complement_ == NULL) { 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); @@ -119,7 +114,7 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( new VisibilityBasedPreconditioner(*A->block_structure(), options_)); } is_preconditioner_good = - visibility_based_preconditioner_->Compute(*A, per_solve_options.D); + visibility_based_preconditioner_->Update(*A, per_solve_options.D); cg_per_solve_options.preconditioner = visibility_based_preconditioner_.get(); break; diff --git a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc deleted file mode 100644 index b40a5162adc..00000000000 --- a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc +++ /dev/null @@ -1,574 +0,0 @@ -// 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_strategy.cc b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt_strategy.cc new file mode 100644 index 00000000000..9e6a59e3813 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt_strategy.cc @@ -0,0 +1,144 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/levenberg_marquardt_strategy.h" + +#include <cmath> +#include "Eigen/Core" +#include "ceres/array_utils.h" +#include "ceres/internal/eigen.h" +#include "ceres/linear_solver.h" +#include "ceres/sparse_matrix.h" +#include "ceres/trust_region_strategy.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +LevenbergMarquardtStrategy::LevenbergMarquardtStrategy( + const TrustRegionStrategy::Options& options) + : linear_solver_(options.linear_solver), + radius_(options.initial_radius), + max_radius_(options.max_radius), + min_diagonal_(options.lm_min_diagonal), + max_diagonal_(options.lm_max_diagonal), + decrease_factor_(2.0), + reuse_diagonal_(false) { + CHECK_NOTNULL(linear_solver_); + CHECK_GT(min_diagonal_, 0.0); + CHECK_LE(min_diagonal_, max_diagonal_); + CHECK_GT(max_radius_, 0.0); +} + +LevenbergMarquardtStrategy::~LevenbergMarquardtStrategy() { +} + +TrustRegionStrategy::Summary LevenbergMarquardtStrategy::ComputeStep( + const TrustRegionStrategy::PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals, + double* step) { + CHECK_NOTNULL(jacobian); + CHECK_NOTNULL(residuals); + CHECK_NOTNULL(step); + + const int num_parameters = jacobian->num_cols(); + if (!reuse_diagonal_) { + if (diagonal_.rows() != num_parameters) { + diagonal_.resize(num_parameters, 1); + } + + jacobian->SquaredColumnNorm(diagonal_.data()); + for (int i = 0; i < num_parameters; ++i) { + diagonal_[i] = min(max(diagonal_[i], min_diagonal_), max_diagonal_); + } + } + + lm_diagonal_ = (diagonal_ / radius_).array().sqrt(); + + LinearSolver::PerSolveOptions solve_options; + solve_options.D = lm_diagonal_.data(); + solve_options.q_tolerance = per_solve_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; + + // Invalidate the output array lm_step, so that we can detect if + // the linear solver generated numerical garbage. This is known + // to happen for the DENSE_QR and then DENSE_SCHUR solver when + // the Jacobin is severly rank deficient and mu is too small. + InvalidateArray(num_parameters, step); + + // Instead of solving Jx = -r, solve Jy = r. + // Then x can be found as x = -y, but the inputs jacobian and residuals + // do not need to be modified. + LinearSolver::Summary linear_solver_summary = + linear_solver_->Solve(jacobian, residuals, solve_options, step); + if (linear_solver_summary.termination_type == FAILURE || + !IsArrayValid(num_parameters, step)) { + LOG(WARNING) << "Linear solver failure. Failed to compute a finite step."; + linear_solver_summary.termination_type = FAILURE; + } else { + VectorRef(step, num_parameters) *= -1.0; + } + + reuse_diagonal_ = true; + + TrustRegionStrategy::Summary summary; + summary.residual_norm = linear_solver_summary.residual_norm; + summary.num_iterations = linear_solver_summary.num_iterations; + summary.termination_type = linear_solver_summary.termination_type; + return summary; +} + +void LevenbergMarquardtStrategy::StepAccepted(double step_quality) { + CHECK_GT(step_quality, 0.0); + radius_ = radius_ / std::max(1.0 / 3.0, + 1.0 - pow(2.0 * step_quality - 1.0, 3)); + radius_ = std::min(max_radius_, radius_); + decrease_factor_ = 2.0; + reuse_diagonal_ = false; +} + +void LevenbergMarquardtStrategy::StepRejected(double step_quality) { + radius_ = radius_ / decrease_factor_; + decrease_factor_ *= 2.0; + reuse_diagonal_ = true; +} + +double LevenbergMarquardtStrategy::Radius() const { + return radius_; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt_strategy.h b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt_strategy.h new file mode 100644 index 00000000000..90c21789797 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt_strategy.h @@ -0,0 +1,86 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_LEVENBERG_MARQUARDT_STRATEGY_H_ +#define CERES_INTERNAL_LEVENBERG_MARQUARDT_STRATEGY_H_ + +#include "ceres/internal/eigen.h" +#include "ceres/trust_region_strategy.h" + +namespace ceres { +namespace internal { + +// Levenberg-Marquardt step computation and trust region sizing +// strategy based on 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 +class LevenbergMarquardtStrategy : public TrustRegionStrategy { +public: + LevenbergMarquardtStrategy(const TrustRegionStrategy::Options& options); + virtual ~LevenbergMarquardtStrategy(); + + // TrustRegionStrategy interface + virtual TrustRegionStrategy::Summary ComputeStep( + const TrustRegionStrategy::PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals, + double* step); + virtual void StepAccepted(double step_quality); + virtual void StepRejected(double step_quality); + virtual void StepIsInvalid() { + // Treat the current step as a rejected step with no increase in + // solution quality. Since rejected steps lead to decrease in the + // size of the trust region, the next time ComputeStep is called, + // this will lead to a better conditioned system. + StepRejected(0.0); + } + + virtual double Radius() const; + + private: + LinearSolver* linear_solver_; + double radius_; + double max_radius_; + const double min_diagonal_; + const double max_diagonal_; + double decrease_factor_; + bool reuse_diagonal_; + Vector diagonal_; // diagonal_ = diag(J'J) + // Scaled copy of diagonal_. Stored here as optimization to prevent + // allocations in every iteration and reuse when a step fails and + // ComputeStep is called again. + Vector lm_diagonal_; // lm_diagonal_ = diagonal_ / radius_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LEVENBERG_MARQUARDT_STRATEGY_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 index cca9f442fe7..3e3bcd0e7eb 100644 --- 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 @@ -33,17 +33,17 @@ #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/internal/scoped_ptr.h" #include "ceres/matrix_proto.h" -#include "ceres/triplet_sparse_matrix.h" #include "ceres/stringprintf.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -64,7 +64,7 @@ LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) { return NULL; } -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( const string& filename) { LinearLeastSquaresProblemProto problem_proto; @@ -130,7 +130,7 @@ LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( << "Ceres to be built with Protocol Buffers support."; return NULL; } -#endif // CERES_DONT_HAVE_PROTOCOL_BUFFERS +#endif // CERES_NO_PROTOCOL_BUFFERS /* A = [1 2] @@ -600,7 +600,7 @@ bool DumpLinearLeastSquaresProblemToConsole(const string& directory, return true; }; -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory, int iteration, const SparseMatrix* A, @@ -681,30 +681,55 @@ bool DumpLinearLeastSquaresProblemToTextFile(const string& directory, string filename_prefix = StringPrintf(format_string.c_str(), iteration); + LOG(INFO) << "writing to: " << filename_prefix << "*"; + + string matlab_script; + StringAppendF(&matlab_script, + "function lsqp = lm_iteration_%03d()\n", iteration); + StringAppendF(&matlab_script, + "lsqp.num_rows = %d;\n", A->num_rows()); + StringAppendF(&matlab_script, + "lsqp.num_cols = %d;\n", A->num_cols()); + { 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); + StringAppendF(&matlab_script, + "tmp = load('%s', '-ascii');\n", filename.c_str()); + StringAppendF( + &matlab_script, + "lsqp.A = sparse(tmp(:, 1) + 1, tmp(:, 2) + 1, tmp(:, 3), %d, %d);\n", + A->num_rows(), + A->num_cols()); } + if (D != NULL) { string filename = filename_prefix + "_D.txt"; WriteArrayToFileOrDie(filename, D, A->num_cols()); + StringAppendF(&matlab_script, + "lsqp.D = load('%s', '-ascii');\n", filename.c_str()); } if (b != NULL) { string filename = filename_prefix + "_b.txt"; WriteArrayToFileOrDie(filename, b, A->num_rows()); + StringAppendF(&matlab_script, + "lsqp.b = load('%s', '-ascii');\n", filename.c_str()); } if (x != NULL) { string filename = filename_prefix + "_x.txt"; WriteArrayToFileOrDie(filename, x, A->num_cols()); + StringAppendF(&matlab_script, + "lsqp.x = load('%s', '-ascii');\n", filename.c_str()); } + string matlab_filename = filename_prefix + ".m"; + WriteStringToFileOrDie(matlab_script, matlab_filename); return true; } diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc index b2e3941eea1..08c3ba110d0 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc @@ -30,13 +30,14 @@ #include "ceres/linear_solver.h" -#include <glog/logging.h> #include "ceres/cgnr_solver.h" +#include "ceres/dense_normal_cholesky_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" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -50,22 +51,24 @@ LinearSolver* LinearSolver::Create(const LinearSolver::Options& options) { return new CgnrSolver(options); case SPARSE_NORMAL_CHOLESKY: -#ifndef CERES_NO_SUITESPARSE - return new SparseNormalCholeskySolver(options); -#else +#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) LOG(WARNING) << "SPARSE_NORMAL_CHOLESKY is not available. Please " - << "build Ceres with SuiteSparse. Returning NULL."; + << "build Ceres with SuiteSparse or CXSparse. " + << "Returning NULL."; return NULL; -#endif // CERES_NO_SUITESPARSE +#else + return new SparseNormalCholeskySolver(options); +#endif case SPARSE_SCHUR: -#ifndef CERES_NO_SUITESPARSE - return new SparseSchurComplementSolver(options); -#else +#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) LOG(WARNING) << "SPARSE_SCHUR is not available. Please " - << "build Ceres with SuiteSparse. Returning NULL."; + << "build Ceres with SuiteSparse or CXSparse. " + << "Returning NULL."; return NULL; -#endif // CERES_NO_SUITESPARSE +#else + return new SparseSchurComplementSolver(options); +#endif case DENSE_SCHUR: return new DenseSchurComplementSolver(options); @@ -76,10 +79,13 @@ LinearSolver* LinearSolver::Create(const LinearSolver::Options& options) { case DENSE_QR: return new DenseQRSolver(options); + case DENSE_NORMAL_CHOLESKY: + return new DenseNormalCholeskySolver(options); + default: LOG(FATAL) << "Unknown linear solver type :" << options.type; - return NULL; // MSVC doesn't understand that LOG(FATAL) never returns. + return NULL; // MSVC doesn't understand that LOG(FATAL) never returns. } } diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h index 5860ecc8a77..31f88740b9f 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h @@ -55,10 +55,11 @@ class LinearOperator; // 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. +// maybe used multiple times for solving multiple linear systems with +// the same sparsity structure. This allows them to cache and reuse +// information across solves. This means that calling Solve on the +// same LinearSolver instance with two different linear systems will +// result in undefined behaviour. // // Subclasses of LinearSolver use two structs to configure themselves. // The Options struct configures the LinearSolver object for its @@ -70,10 +71,11 @@ class LinearSolver { Options() : type(SPARSE_NORMAL_CHOLESKY), preconditioner_type(JACOBI), + sparse_linear_algebra_library(SUITE_SPARSE), + use_block_amd(true), 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), @@ -85,6 +87,11 @@ class LinearSolver { PreconditionerType preconditioner_type; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library; + + // See solver.h for explanation of this option. + bool use_block_amd; + // Number of internal iterations that the solver uses. This // parameter only makes sense for iterative solvers like CG. int min_num_iterations; @@ -93,10 +100,6 @@ class LinearSolver { // 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 @@ -121,8 +124,8 @@ class LinearSolver { // It is expected that these parameters are set programmatically // rather than manually. // - // Please see explicit_schur_complement_solver_impl.h for more - // details. + // Please see schur_complement_solver.h and schur_eliminator.h for + // more details. int row_block_size; int e_block_size; int f_block_size; @@ -244,6 +247,7 @@ class LinearSolver { const PerSolveOptions& per_solve_options, double* x) = 0; + // Factory static LinearSolver* Create(const Options& options); }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc b/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc index eeae74e3f95..26e7f4908a4 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc @@ -28,10 +28,11 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) -#include <glog/logging.h> -#include "ceres/internal/eigen.h" #include "ceres/local_parameterization.h" + +#include "ceres/internal/eigen.h" #include "ceres/rotation.h" +#include "glog/logging.h" 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 index 00b2b184729..b948f289f21 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc @@ -77,6 +77,70 @@ void CauchyLoss::Evaluate(double s, double rho[3]) const { rho[2] = - c_ * (inv * inv); } +void ArctanLoss::Evaluate(double s, double rho[3]) const { + const double sum = 1 + s * s * b_; + const double inv = 1 / sum; + // 'sum' and 'inv' are always positive. + rho[0] = a_ * atan2(s, a_); + rho[1] = inv; + rho[2] = -2 * s * b_ * (inv * inv); +} + +TolerantLoss::TolerantLoss(double a, double b) + : a_(a), + b_(b), + c_(b * log(1.0 + exp(-a / b))) { + CHECK_GE(a, 0.0); + CHECK_GT(b, 0.0); +} + +void TolerantLoss::Evaluate(double s, double rho[3]) const { + const double x = (s - a_) / b_; + // The basic equation is rho[0] = b ln(1 + e^x). However, if e^x is too + // large, it will overflow. Since numerically 1 + e^x == e^x when the + // x is greater than about ln(2^53) for doubles, beyond this threshold + // we substitute x for ln(1 + e^x) as a numerically equivalent approximation. + static const double kLog2Pow53 = 36.7; // ln(MathLimits<double>::kEpsilon). + if (x > kLog2Pow53) { + rho[0] = s - a_ - c_; + rho[1] = 1.0; + rho[2] = 0.0; + } else { + const double e_x = exp(x); + rho[0] = b_ * log(1.0 + e_x) - c_; + rho[1] = e_x / (1.0 + e_x); + rho[2] = 0.5 / (b_ * (1.0 + cosh(x))); + } +} + +ComposedLoss::ComposedLoss(const LossFunction* f, Ownership ownership_f, + const LossFunction* g, Ownership ownership_g) + : f_(CHECK_NOTNULL(f)), + g_(CHECK_NOTNULL(g)), + ownership_f_(ownership_f), + ownership_g_(ownership_g) { +} + +ComposedLoss::~ComposedLoss() { + if (ownership_f_ == DO_NOT_TAKE_OWNERSHIP) { + f_.release(); + } + if (ownership_g_ == DO_NOT_TAKE_OWNERSHIP) { + g_.release(); + } +} + +void ComposedLoss::Evaluate(double s, double rho[3]) const { + double rho_f[3], rho_g[3]; + g_->Evaluate(s, rho_g); + f_->Evaluate(rho_g[0], rho_f); + rho[0] = rho_f[0]; + // f'(g(s)) * g'(s). + rho[1] = rho_f[1] * rho_g[1]; + // f''(g(s)) * g'(s) * g'(s) + f'(g(s)) * g''(s). + rho[2] = rho_f[2] * rho_g[1] * rho_g[1] + rho_f[1] * rho_g[2]; +} + void ScaledLoss::Evaluate(double s, double rho[3]) const { if (rho_.get() == NULL) { rho[0] = a_ * s; diff --git a/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h b/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h index b8a3a1a6de6..94b3076e3d7 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h +++ b/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h @@ -33,7 +33,7 @@ #ifndef CERES_INTERNAL_MATRIX_PROTO_H_ #define CERES_INTERNAL_MATRIX_PROTO_H_ -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS #include "ceres/matrix.pb.h" #endif diff --git a/extern/libmv/third_party/ceres/internal/ceres/minimizer.h b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h index 77cb00cb6b4..cfc98a3ebd0 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/minimizer.h +++ b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h @@ -40,6 +40,8 @@ namespace internal { class Evaluator; class LinearSolver; +class SparseMatrix; +class TrustRegionStrategy; // Interface for non-linear least squares solvers. class Minimizer { @@ -48,53 +50,93 @@ class Minimizer { // see solver.h for detailed information about the meaning and // default values of each of these parameters. struct Options { + Options() { + Init(Solver::Options()); + } + explicit Options(const Solver::Options& options) { + Init(options); + } + + void Init(const Solver::Options& options) { max_num_iterations = options.max_num_iterations; - max_solver_time_sec = options.max_solver_time_sec; + max_solver_time_in_seconds = options.max_solver_time_in_seconds; + max_step_solver_retries = 5; 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; + use_nonmonotonic_steps = options.use_nonmonotonic_steps; + max_consecutive_nonmonotonic_steps = + options.max_consecutive_nonmonotonic_steps; 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; + max_num_consecutive_invalid_steps = + options.max_num_consecutive_invalid_steps; + min_trust_region_radius = options.min_trust_region_radius; + evaluator = NULL; + trust_region_strategy = NULL; + jacobian = NULL; + callbacks = options.callbacks; } int max_num_iterations; - int max_solver_time_sec; + double max_solver_time_in_seconds; + + // 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. + int max_step_solver_retries; 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; + bool use_nonmonotonic_steps; + int max_consecutive_nonmonotonic_steps; vector<int> lsqp_iterations_to_dump; DumpFormatType lsqp_dump_format_type; string lsqp_dump_directory; int num_eliminate_blocks; - LoggingType logging_type; + int max_num_consecutive_invalid_steps; + int min_trust_region_radius; // List of callbacks that are executed by the Minimizer at the end // of each iteration. // - // Client owns these pointers. + // The Options struct does not own these pointers. vector<IterationCallback*> callbacks; + + // Object responsible for evaluating the cost, residuals and + // Jacobian matrix. The Options struct does not own this pointer. + Evaluator* evaluator; + + // Object responsible for actually computing the trust region + // step, and sizing the trust region radius. The Options struct + // does not own this pointer. + TrustRegionStrategy* trust_region_strategy; + + // Object holding the Jacobian matrix. It is assumed that the + // sparsity structure of the matrix has already been initialized + // and will remain constant for the life time of the + // optimization. The Options struct does not own this pointer. + SparseMatrix* jacobian; }; virtual ~Minimizer() {} + + // Note: The minimizer is expected to update the state of the + // parameters array every iteration. This is required for the + // StateUpdatingCallback to work. virtual void Minimize(const Options& options, - Evaluator* evaluator, - LinearSolver* linear_solver, - const double* initial_parameters, - double* final_parameters, + double* parameters, Solver::Summary* summary) = 0; }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/mutex.h b/extern/libmv/third_party/ceres/internal/ceres/mutex.h index 6514b107041..5090a71b78d 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/mutex.h +++ b/extern/libmv/third_party/ceres/internal/ceres/mutex.h @@ -95,11 +95,11 @@ #ifndef CERES_INTERNAL_MUTEX_H_ #define CERES_INTERNAL_MUTEX_H_ -#if defined(NO_THREADS) +#if defined(CERES_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 +# define CERES_WIN32_LEAN_AND_MEAN // We only need minimal includes +# ifdef CERES_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. @@ -108,9 +108,9 @@ # endif # endif // To avoid macro definition of ERROR. -# define NOGDI +# define CERES_NOGDI // To avoid macro definition of min/max. -# define NOMINMAX +# define CERES_NOMINMAX # include <windows.h> typedef CRITICAL_SECTION MutexType; #elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK) @@ -151,7 +151,7 @@ class Mutex { inline void Lock(); // Block if needed until free then acquire exclusively inline void Unlock(); // Release a lock acquired via Lock() -#ifdef GMUTEX_TRYLOCK +#ifdef CERES_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 @@ -183,7 +183,7 @@ class Mutex { }; // Now the implementation of Mutex for various systems -#if defined(NO_THREADS) +#if defined(CERES_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 @@ -199,7 +199,7 @@ Mutex::Mutex() : mutex_(0) { } Mutex::~Mutex() { assert(mutex_ == 0); } void Mutex::Lock() { assert(--mutex_ == -1); } void Mutex::Unlock() { assert(mutex_++ == -1); } -#ifdef GMUTEX_TRYLOCK +#ifdef CERES_GMUTEX_TRYLOCK bool Mutex::TryLock() { if (mutex_) return false; Lock(); return true; } #endif void Mutex::ReaderLock() { assert(++mutex_ > 0); } @@ -220,91 +220,101 @@ 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(); \ +#define CERES_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 +Mutex::~Mutex() { CERES_SAFE_PTHREAD(pthread_rwlock_destroy); } +void Mutex::Lock() { CERES_SAFE_PTHREAD(pthread_rwlock_wrlock); } +void Mutex::Unlock() { CERES_SAFE_PTHREAD(pthread_rwlock_unlock); } +#ifdef CERES_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 +void Mutex::ReaderLock() { CERES_SAFE_PTHREAD(pthread_rwlock_rdlock); } +void Mutex::ReaderUnlock() { CERES_SAFE_PTHREAD(pthread_rwlock_unlock); } +#undef CERES_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(); \ +#define CERES_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 +Mutex::~Mutex() { CERES_SAFE_PTHREAD(pthread_mutex_destroy); } +void Mutex::Lock() { CERES_SAFE_PTHREAD(pthread_mutex_lock); } +void Mutex::Unlock() { CERES_SAFE_PTHREAD(pthread_mutex_unlock); } +#ifdef CERES_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 +#undef CERES_SAFE_PTHREAD #endif // -------------------------------------------------------------------------- // Some helper classes -// MutexLock(mu) acquires mu when constructed and releases it when destroyed. -class MutexLock { +// Note: The weird "Ceres" prefix for the class is a workaround for having two +// similar mutex.h files included in the same translation unit. This is a +// problem because macros do not respect C++ namespaces, and as a result, this +// does not work well (e.g. inside Chrome). The offending macros are +// "MutexLock(x) COMPILE_ASSERT(false)". To work around this, "Ceres" is +// prefixed to the class names; this permits defining the classes. + +// CeresMutexLock(mu) acquires mu when constructed and releases it when destroyed. +class CeresMutexLock { public: - explicit MutexLock(Mutex *mu) : mu_(mu) { mu_->Lock(); } - ~MutexLock() { mu_->Unlock(); } + explicit CeresMutexLock(Mutex *mu) : mu_(mu) { mu_->Lock(); } + ~CeresMutexLock() { mu_->Unlock(); } private: Mutex * const mu_; // Disallow "evil" constructors - MutexLock(const MutexLock&); - void operator=(const MutexLock&); + CeresMutexLock(const CeresMutexLock&); + void operator=(const CeresMutexLock&); }; -// ReaderMutexLock and WriterMutexLock do the same, for rwlocks -class ReaderMutexLock { +// CeresReaderMutexLock and CeresWriterMutexLock do the same, for rwlocks +class CeresReaderMutexLock { public: - explicit ReaderMutexLock(Mutex *mu) : mu_(mu) { mu_->ReaderLock(); } - ~ReaderMutexLock() { mu_->ReaderUnlock(); } + explicit CeresReaderMutexLock(Mutex *mu) : mu_(mu) { mu_->ReaderLock(); } + ~CeresReaderMutexLock() { mu_->ReaderUnlock(); } private: Mutex * const mu_; // Disallow "evil" constructors - ReaderMutexLock(const ReaderMutexLock&); - void operator=(const ReaderMutexLock&); + CeresReaderMutexLock(const CeresReaderMutexLock&); + void operator=(const CeresReaderMutexLock&); }; -class WriterMutexLock { +class CeresWriterMutexLock { public: - explicit WriterMutexLock(Mutex *mu) : mu_(mu) { mu_->WriterLock(); } - ~WriterMutexLock() { mu_->WriterUnlock(); } + explicit CeresWriterMutexLock(Mutex *mu) : mu_(mu) { mu_->WriterLock(); } + ~CeresWriterMutexLock() { mu_->WriterUnlock(); } private: Mutex * const mu_; // Disallow "evil" constructors - WriterMutexLock(const WriterMutexLock&); - void operator=(const WriterMutexLock&); + CeresWriterMutexLock(const CeresWriterMutexLock&); + void operator=(const CeresWriterMutexLock&); }; // 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) +#define CeresMutexLock(x) \ + COMPILE_ASSERT(0, ceres_mutex_lock_decl_missing_var_name) +#define CeresReaderMutexLock(x) \ + COMPILE_ASSERT(0, ceres_rmutex_lock_decl_missing_var_name) +#define CeresWriterMutexLock(x) \ + COMPILE_ASSERT(0, ceres_wmutex_lock_decl_missing_var_name) } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc b/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc index f30bbc8b46b..392d728fb32 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc @@ -32,11 +32,10 @@ #include <cstddef> #include <vector> - -#include <glog/logging.h> #include "ceres/internal/eigen.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" +#include "glog/logging.h" 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 index 4bac1a85828..f20805ca873 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h +++ b/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h @@ -32,13 +32,15 @@ #define CERES_INTERNAL_PARAMETER_BLOCK_H_ #include <cstdlib> +#include <string> +#include "ceres/array_utils.h" #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" +#include "ceres/stringprintf.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -172,6 +174,19 @@ class ParameterBlock { return local_parameterization_->Plus(x, delta, x_plus_delta); } + string ToString() const { + return StringPrintf("{ user_state=%p, state=%p, size=%d, " + "constant=%d, index=%d, state_offset=%d, " + "delta_offset=%d }", + user_state_, + state_, + size_, + is_constant_, + index_, + state_offset_, + delta_offset_); + } + private: void Init(double* user_state, int size, 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 index fcf8fd53aed..0722fc82c02 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc @@ -35,10 +35,10 @@ #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" +#include "glog/logging.h" namespace ceres { namespace internal { diff --git a/extern/libmv/third_party/ceres/internal/ceres/polynomial_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/polynomial_solver.cc new file mode 100644 index 00000000000..20c01566a89 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/polynomial_solver.cc @@ -0,0 +1,184 @@ +// 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: moll.markus@arcor.de (Markus Moll) + +#include "ceres/polynomial_solver.h" + +#include <cmath> +#include <cstddef> +#include "Eigen/Dense" +#include "ceres/internal/port.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { + +// Balancing function as described by B. N. Parlett and C. Reinsch, +// "Balancing a Matrix for Calculation of Eigenvalues and Eigenvectors". +// In: Numerische Mathematik, Volume 13, Number 4 (1969), 293-304, +// Springer Berlin / Heidelberg. DOI: 10.1007/BF02165404 +void BalanceCompanionMatrix(Matrix* companion_matrix_ptr) { + CHECK_NOTNULL(companion_matrix_ptr); + Matrix& companion_matrix = *companion_matrix_ptr; + Matrix companion_matrix_offdiagonal = companion_matrix; + companion_matrix_offdiagonal.diagonal().setZero(); + + const int degree = companion_matrix.rows(); + + // gamma <= 1 controls how much a change in the scaling has to + // lower the 1-norm of the companion matrix to be accepted. + // + // gamma = 1 seems to lead to cycles (numerical issues?), so + // we set it slightly lower. + const double gamma = 0.9; + + // Greedily scale row/column pairs until there is no change. + bool scaling_has_changed; + do { + scaling_has_changed = false; + + for (int i = 0; i < degree; ++i) { + const double row_norm = companion_matrix_offdiagonal.row(i).lpNorm<1>(); + const double col_norm = companion_matrix_offdiagonal.col(i).lpNorm<1>(); + + // Decompose row_norm/col_norm into mantissa * 2^exponent, + // where 0.5 <= mantissa < 1. Discard mantissa (return value + // of frexp), as only the exponent is needed. + int exponent = 0; + std::frexp(row_norm / col_norm, &exponent); + exponent /= 2; + + if (exponent != 0) { + const double scaled_col_norm = std::ldexp(col_norm, exponent); + const double scaled_row_norm = std::ldexp(row_norm, -exponent); + if (scaled_col_norm + scaled_row_norm < gamma * (col_norm + row_norm)) { + // Accept the new scaling. (Multiplication by powers of 2 should not + // introduce rounding errors (ignoring non-normalized numbers and + // over- or underflow)) + scaling_has_changed = true; + companion_matrix_offdiagonal.row(i) *= std::ldexp(1.0, -exponent); + companion_matrix_offdiagonal.col(i) *= std::ldexp(1.0, exponent); + } + } + } + } while (scaling_has_changed); + + companion_matrix_offdiagonal.diagonal() = companion_matrix.diagonal(); + companion_matrix = companion_matrix_offdiagonal; + VLOG(3) << "Balanced companion matrix is\n" << companion_matrix; +} + +void BuildCompanionMatrix(const Vector& polynomial, + Matrix* companion_matrix_ptr) { + CHECK_NOTNULL(companion_matrix_ptr); + Matrix& companion_matrix = *companion_matrix_ptr; + + const int degree = polynomial.size() - 1; + + companion_matrix.resize(degree, degree); + companion_matrix.setZero(); + companion_matrix.diagonal(-1).setOnes(); + companion_matrix.col(degree - 1) = -polynomial.reverse().head(degree); +} + +// Remove leading terms with zero coefficients. +Vector RemoveLeadingZeros(const Vector& polynomial_in) { + int i = 0; + while (i < (polynomial_in.size() - 1) && polynomial_in(i) == 0.0) { + ++i; + } + return polynomial_in.tail(polynomial_in.size() - i); +} +} // namespace + +bool FindPolynomialRoots(const Vector& polynomial_in, + Vector* real, + Vector* imaginary) { + if (polynomial_in.size() == 0) { + LOG(ERROR) << "Invalid polynomial of size 0 passed to FindPolynomialRoots"; + return false; + } + + Vector polynomial = RemoveLeadingZeros(polynomial_in); + const int degree = polynomial.size() - 1; + + // Is the polynomial constant? + if (degree == 0) { + LOG(WARNING) << "Trying to extract roots from a constant " + << "polynomial in FindPolynomialRoots"; + return true; + } + + // Divide by leading term + const double leading_term = polynomial(0); + polynomial /= leading_term; + + // Separately handle linear polynomials. + if (degree == 1) { + if (real != NULL) { + real->resize(1); + (*real)(0) = -polynomial(1); + } + if (imaginary != NULL) { + imaginary->resize(1); + imaginary->setZero(); + } + } + + // The degree is now known to be at least 2. + // Build and balance the companion matrix to the polynomial. + Matrix companion_matrix(degree, degree); + BuildCompanionMatrix(polynomial, &companion_matrix); + BalanceCompanionMatrix(&companion_matrix); + + // Find its (complex) eigenvalues. + Eigen::EigenSolver<Matrix> solver(companion_matrix, + Eigen::EigenvaluesOnly); + if (solver.info() != Eigen::Success) { + LOG(ERROR) << "Failed to extract eigenvalues from companion matrix."; + return false; + } + + // Output roots + if (real != NULL) { + *real = solver.eigenvalues().real(); + } else { + LOG(WARNING) << "NULL pointer passed as real argument to " + << "FindPolynomialRoots. Real parts of the roots will not " + << "be returned."; + } + if (imaginary != NULL) { + *imaginary = solver.eigenvalues().imag(); + } + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/polynomial_solver.h b/extern/libmv/third_party/ceres/internal/ceres/polynomial_solver.h new file mode 100644 index 00000000000..1cf07ddb549 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/polynomial_solver.h @@ -0,0 +1,65 @@ +// 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: moll.markus@arcor.de (Markus Moll) + +#ifndef CERES_INTERNAL_POLYNOMIAL_SOLVER_H_ +#define CERES_INTERNAL_POLYNOMIAL_SOLVER_H_ + +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +// Use the companion matrix eigenvalues to determine the roots of the polynomial +// +// sum_{i=0}^N polynomial(i) x^{N-i}. +// +// This function returns true on success, false otherwise. +// Failure indicates that the polynomial is invalid (of size 0) or +// that the eigenvalues of the companion matrix could not be computed. +// On failure, a more detailed message will be written to LOG(ERROR). +// If real is not NULL, the real parts of the roots will be returned in it. +// Likewise, if imaginary is not NULL, imaginary parts will be returned in it. +bool FindPolynomialRoots(const Vector& polynomial, + Vector* real, + Vector* imaginary); + +// Evaluate the polynomial at x using the Horner scheme. +inline double EvaluatePolynomial(const Vector& polynomial, double x) { + double v = 0.0; + for (int i = 0; i < polynomial.size(); ++i) { + v = v * x + polynomial(i); + } + return v; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_POLYNOMIAL_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc index 68242477d6f..c186f527be8 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc @@ -37,16 +37,15 @@ #include <string> #include <utility> #include <vector> - -#include <glog/logging.h> +#include "ceres/cost_function.h" +#include "ceres/loss_function.h" +#include "ceres/map_util.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" +#include "glog/logging.h" namespace ceres { namespace internal { diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h index 523860e652a..2ca055448c3 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h @@ -118,7 +118,7 @@ class ProblemImpl { map<double*, ParameterBlock*> parameter_block_map_; internal::scoped_ptr<internal::Program> program_; - DISALLOW_COPY_AND_ASSIGN(ProblemImpl); + CERES_DISALLOW_COPY_AND_ASSIGN(ProblemImpl); }; } // namespace internal diff --git a/extern/libmv/third_party/ceres/internal/ceres/program.cc b/extern/libmv/third_party/ceres/internal/ceres/program.cc index 444b1020253..82d76d39233 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/program.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/program.cc @@ -32,14 +32,18 @@ #include <map> #include <vector> +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/cost_function.h" +#include "ceres/evaluator.h" +#include "ceres/internal/port.h" +#include "ceres/local_parameterization.h" +#include "ceres/loss_function.h" +#include "ceres/map_util.h" #include "ceres/parameter_block.h" +#include "ceres/problem.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 { @@ -69,7 +73,8 @@ vector<ResidualBlock*>* Program::mutable_residual_blocks() { bool Program::StateVectorToParameterBlocks(const double *state) { for (int i = 0; i < parameter_blocks_.size(); ++i) { - if (!parameter_blocks_[i]->SetState(state)) { + if (!parameter_blocks_[i]->IsConstant() && + !parameter_blocks_[i]->SetState(state)) { return false; } state += parameter_blocks_[i]->Size(); @@ -86,9 +91,18 @@ void Program::ParameterBlocksToStateVector(double *state) const { void Program::CopyParameterBlockStateToUserState() { for (int i = 0; i < parameter_blocks_.size(); ++i) { - parameter_blocks_[i]->GetState( - parameter_blocks_[i]->mutable_user_state()); + parameter_blocks_[i]->GetState(parameter_blocks_[i]->mutable_user_state()); + } +} + +bool Program::SetParameterBlockStatePtrsToUserStatePtrs() { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + if (!parameter_blocks_[i]->IsConstant() && + !parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) { + return false; + } } + return true; } bool Program::Plus(const double* state, @@ -193,40 +207,25 @@ int Program::MaxParametersPerResidualBlock() const { 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(); - } - +int Program::MaxResidualsPerResidualBlock() const { + int max_residuals = 0; 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; + max_residuals = max(max_residuals, + residual_blocks_[i]->NumResiduals()); + } + return max_residuals; +} - // Update the residuals cursor. - if (residuals != NULL) { - residuals += residual_block->NumResiduals(); - } +string Program::ToString() const { + string ret = "Program dump\n"; + ret += StringPrintf("Number of parameter blocks: %d\n", NumParameterBlocks()); + ret += StringPrintf("Number of parameters: %d\n", NumParameters()); + ret += "Parameters:\n"; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + ret += StringPrintf("%d: %s\n", + i, parameter_blocks_[i]->ToString().c_str()); } - return true; + return ret; } } // namespace internal diff --git a/extern/libmv/third_party/ceres/internal/ceres/program.h b/extern/libmv/third_party/ceres/internal/ceres/program.h index 113d352d562..5002b7e752e 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/program.h +++ b/extern/libmv/third_party/ceres/internal/ceres/program.h @@ -31,6 +31,7 @@ #ifndef CERES_INTERNAL_PROGRAM_H_ #define CERES_INTERNAL_PROGRAM_H_ +#include <string> #include <vector> #include "ceres/internal/port.h" @@ -71,9 +72,14 @@ class Program { bool StateVectorToParameterBlocks(const double *state); void ParameterBlocksToStateVector(double *state) const; - // Copy internal state out to the user's parameters. + // Copy internal state to the user's parameters. void CopyParameterBlockStateToUserState(); + // Set the parameter block pointers to the user pointers. Since this + // runs parameter block set state internally, which may call local + // parameterizations, this can fail. False is returned on failure. + bool SetParameterBlockStatePtrsToUserStatePtrs(); + // Update a state vector for the program given a delta. bool Plus(const double* state, const double* delta, @@ -103,16 +109,11 @@ class Program { int MaxScratchDoublesNeededForEvaluate() const; int MaxDerivativesPerResidualBlock() const; int MaxParametersPerResidualBlock() const; + int MaxResidualsPerResidualBlock() 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); + // A human-readable dump of the parameter blocks for debugging. + // TODO(keir): If necessary, also dump the residual blocks. + string ToString() const; private: // The Program does not own the ParameterBlock or ResidualBlock objects. diff --git a/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h b/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h index 7ec74b1b269..6c48e7d7643 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h +++ b/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h @@ -120,13 +120,18 @@ class ProgramEvaluator : public Evaluator { bool Evaluate(const double* state, double* cost, double* residuals, + double* gradient, SparseMatrix* jacobian) { // The parameters are stateful, so set the state before evaluating. if (!program_->StateVectorToParameterBlocks(state)) { return false; } - if (jacobian) { + if (residuals != NULL) { + VectorRef(residuals, program_->NumResiduals()).setZero(); + } + + if (jacobian != NULL) { jacobian->SetZero(); } @@ -158,13 +163,16 @@ class ProgramEvaluator : public Evaluator { // Prepare block residuals if requested. const ResidualBlock* residual_block = program_->residual_blocks()[i]; - double* block_residuals = (residuals != NULL) - ? (residuals + residual_layout_[i]) - : NULL; + double* block_residuals = NULL; + if (residuals != NULL) { + block_residuals = residuals + residual_layout_[i]; + } else if (gradient != NULL) { + block_residuals = scratch->residual_block_residuals.get(); + } // Prepare block jacobians if requested. double** block_jacobians = NULL; - if (jacobian != NULL) { + if (jacobian != NULL || gradient != NULL) { preparer->Prepare(residual_block, i, jacobian, @@ -174,10 +182,11 @@ class ProgramEvaluator : public Evaluator { // Evaluate the cost, residuals, and jacobians. double block_cost; - if (!residual_block->Evaluate(&block_cost, - block_residuals, - block_jacobians, - scratch->scratch.get())) { + if (!residual_block->Evaluate( + &block_cost, + block_residuals, + block_jacobians, + scratch->residual_block_evaluate_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 @@ -188,19 +197,49 @@ class ProgramEvaluator : public Evaluator { scratch->cost += block_cost; + // Store the jacobians, if they were requested. if (jacobian != NULL) { jacobian_writer_.Write(i, residual_layout_[i], block_jacobians, jacobian); } + + // Compute and store the gradient, if it was requested. + if (gradient != NULL) { + 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()) { + continue; + } + MatrixRef block_jacobian(block_jacobians[j], + num_residuals, + parameter_block->LocalSize()); + VectorRef block_gradient(scratch->gradient.get() + + parameter_block->delta_offset(), + parameter_block->LocalSize()); + VectorRef block_residual(block_residuals, num_residuals); + block_gradient += block_residual.transpose() * block_jacobian; + } + } } if (!abort) { - // Sum the cost from each thread. + // Sum the cost and gradient (if requested) from each thread. (*cost) = 0.0; + int num_parameters = program_->NumEffectiveParameters(); + if (gradient != NULL) { + VectorRef(gradient, num_parameters).setZero(); + } for (int i = 0; i < options_.num_threads; ++i) { (*cost) += evaluate_scratch_[i].cost; + if (gradient != NULL) { + VectorRef(gradient, num_parameters) += + VectorRef(evaluate_scratch_[i].gradient.get(), num_parameters); + } } } return !abort; @@ -224,16 +263,28 @@ class ProgramEvaluator : public Evaluator { } private: + // Per-thread scratch space needed to evaluate and store each residual block. struct EvaluateScratch { void Init(int max_parameters_per_residual_block, - int max_scratch_doubles_needed_for_evaluate) { + int max_scratch_doubles_needed_for_evaluate, + int max_residuals_per_residual_block, + int num_parameters) { + residual_block_evaluate_scratch.reset( + new double[max_scratch_doubles_needed_for_evaluate]); + gradient.reset(new double[num_parameters]); + VectorRef(gradient.get(), num_parameters).setZero(); + residual_block_residuals.reset( + new double[max_residuals_per_residual_block]); 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> residual_block_evaluate_scratch; + // The gradient in the local parameterization. + scoped_array<double> gradient; + // Enough space to store the residual for the largest residual block. + scoped_array<double> residual_block_residuals; scoped_array<double*> jacobian_block_ptrs; }; @@ -256,11 +307,16 @@ class ProgramEvaluator : public Evaluator { program.MaxParametersPerResidualBlock(); int max_scratch_doubles_needed_for_evaluate = program.MaxScratchDoublesNeededForEvaluate(); + int max_residuals_per_residual_block = + program.MaxResidualsPerResidualBlock(); + int num_parameters = program.NumEffectiveParameters(); 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); + max_scratch_doubles_needed_for_evaluate, + max_residuals_per_residual_block, + num_parameters); } return evaluate_scratch; } diff --git a/extern/libmv/third_party/ceres/internal/ceres/random.h b/extern/libmv/third_party/ceres/internal/ceres/random.h index 769e0b4dd27..352c0032b5a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/random.h +++ b/extern/libmv/third_party/ceres/internal/ceres/random.h @@ -27,21 +27,44 @@ // POSSIBILITY OF SUCH DAMAGE. // // Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) #ifndef CERES_INTERNAL_RANDOM_H_ #define CERES_INTERNAL_RANDOM_H_ +#include <cmath> +#include <cstdlib> +#include "ceres/internal/port.h" + namespace ceres { -inline double RandDouble() { - double r = rand(); - return r / RAND_MAX; +inline void SetRandomState(int state) { + srand(state); } inline int Uniform(int n) { return rand() % n; } +inline double RandDouble() { + double r = static_cast<double>(rand()); + return r / RAND_MAX; +} + +// Box-Muller algorithm for normal random number generation. +// http://en.wikipedia.org/wiki/Box-Muller_transform +inline double RandNormal() { + double x1, x2, w; + do { + x1 = 2.0 * RandDouble() - 1.0; + x2 = 2.0 * RandDouble() - 1.0; + w = x1 * x1 + x2 * x2; + } while ( w >= 1.0 || w == 0.0 ); + + w = sqrt((-2.0 * log(w)) / w); + return x1 * w; +} + } // 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 index 03867891dba..bdb88b1dd97 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc @@ -102,8 +102,11 @@ bool ResidualBlock::Evaluate(double* cost, InvalidateEvaluation(*this, cost, residuals, eval_jacobians); - if (!cost_function_->Evaluate(parameters.get(), residuals, eval_jacobians) || - !IsEvaluationValid(*this, + if (!cost_function_->Evaluate(parameters.get(), residuals, eval_jacobians)) { + return false; + } + + if (!IsEvaluationValid(*this, parameters.get(), cost, residuals, 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 index 28e03130844..ff18e21f024 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.cc @@ -33,46 +33,17 @@ #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/array_utils.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" - -#ifdef _MSC_VER -# define isfinite _finite -#endif +#include "ceres/parameter_block.h" +#include "ceres/residual_block.h" +#include "ceres/stringprintf.h" +#include "glog/logging.h" 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, 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 index 228867cc60c..7051c2112fd 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.h +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.h @@ -51,15 +51,6 @@ 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, 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 index ac6d8aa279a..7af275c1dd8 100644 --- 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 @@ -35,11 +35,10 @@ #include <algorithm> #include <numeric> #include <vector> - -#include <glog/logging.h> #include "Eigen/Dense" #include "ceres/cost_function.h" #include "ceres/internal/scoped_ptr.h" +#include "glog/logging.h" namespace ceres { namespace internal { 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 index 2bc8cdd6bec..b9224d881ce 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc @@ -32,6 +32,11 @@ #include <ctime> #include <set> #include <vector> + +#ifndef CERES_NO_CXSPARSE +#include "cs.h" +#endif // CERES_NO_CXSPARSE + #include "Eigen/Dense" #include "ceres/block_random_access_dense_matrix.h" #include "ceres/block_random_access_matrix.h" @@ -48,6 +53,7 @@ #include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" + namespace ceres { namespace internal { @@ -57,7 +63,7 @@ LinearSolver::Summary SchurComplementSolver::SolveImpl( const LinearSolver::PerSolveOptions& per_solve_options, double* x) { const time_t start_time = time(NULL); - if (!options_.constant_sparsity || (eliminator_.get() == NULL)) { + if (eliminator_.get() == NULL) { InitStorage(A->block_structure()); DetectStructure(*A->block_structure(), options_.num_eliminate_blocks, @@ -88,11 +94,11 @@ LinearSolver::Summary SchurComplementSolver::SolveImpl( 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; + 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; } @@ -139,18 +145,33 @@ bool DenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { return true; } -#ifndef CERES_NO_SUITESPARSE + SparseSchurComplementSolver::SparseSchurComplementSolver( const LinearSolver::Options& options) - : SchurComplementSolver(options), - symbolic_factor_(NULL) { + : SchurComplementSolver(options) { +#ifndef CERES_NO_SUITESPARSE + factor_ = NULL; +#endif // CERES_NO_SUITESPARSE + +#ifndef CERES_NO_CXSPARSE + cxsparse_factor_ = NULL; +#endif // CERES_NO_CXSPARSE } SparseSchurComplementSolver::~SparseSchurComplementSolver() { - if (symbolic_factor_ != NULL) { - ss_.Free(symbolic_factor_); - symbolic_factor_ = NULL; +#ifndef CERES_NO_SUITESPARSE + if (factor_ != NULL) { + ss_.Free(factor_); + factor_ = NULL; } +#endif // CERES_NO_SUITESPARSE + +#ifndef CERES_NO_CXSPARSE + if (cxsparse_factor_ != NULL) { + cxsparse_.Free(cxsparse_factor_); + cxsparse_factor_ = NULL; + } +#endif // CERES_NO_CXSPARSE } // Determine the non-zero blocks in the Schur Complement matrix, and @@ -161,13 +182,13 @@ void SparseSchurComplementSolver::InitStorage( 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); + blocks_.resize(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; + blocks_[i - num_eliminate_blocks] = bs->cols[i].size; } set<pair<int, int> > block_pairs; - for (int i = 0; i < blocks.size(); ++i) { + for (int i = 0; i < blocks_.size(); ++i) { block_pairs.insert(make_pair(i, i)); } @@ -220,15 +241,34 @@ void SparseSchurComplementSolver::InitStorage( } } - set_lhs(new BlockRandomAccessSparseMatrix(blocks, block_pairs)); + set_lhs(new BlockRandomAccessSparseMatrix(blocks_, block_pairs)); set_rhs(new double[lhs()->num_rows()]); } +bool SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { + switch (options().sparse_linear_algebra_library) { + case SUITE_SPARSE: + return SolveReducedLinearSystemUsingSuiteSparse(solution); + case CX_SPARSE: + return SolveReducedLinearSystemUsingCXSparse(solution); + default: + LOG(FATAL) << "Unknown sparse linear algebra library : " + << options().sparse_linear_algebra_library; + } + + LOG(FATAL) << "Unknown sparse linear algebra library : " + << options().sparse_linear_algebra_library; + return false; +} + +#ifndef CERES_NO_SUITESPARSE // 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. +bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( + double* solution) { + const time_t start_time = time(NULL); + TripletSparseMatrix* tsm = const_cast<TripletSparseMatrix*>( down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); @@ -245,30 +285,38 @@ bool SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { // The matrix is symmetric, and the upper triangular part of the // matrix contains the values. cholmod_lhs->stype = 1; + const time_t lhs_time = time(NULL); cholmod_dense* cholmod_rhs = ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows); + const time_t rhs_time = time(NULL); // Symbolic factorization is computed if we don't already have one handy. - if (symbolic_factor_ == NULL) { - symbolic_factor_ = ss_.AnalyzeCholesky(cholmod_lhs); + if (factor_ == NULL) { + if (options().use_block_amd) { + factor_ = ss_.BlockAnalyzeCholesky(cholmod_lhs, blocks_, blocks_); + } else { + factor_ = ss_.AnalyzeCholesky(cholmod_lhs); + } + + if (VLOG_IS_ON(2)) { + cholmod_print_common("Symbolic Analysis", ss_.mutable_cc()); + } } + CHECK_NOTNULL(factor_); + + const time_t symbolic_time = time(NULL); cholmod_dense* cholmod_solution = - ss_.SolveCholesky(cholmod_lhs, symbolic_factor_, cholmod_rhs); + ss_.SolveCholesky(cholmod_lhs, factor_, cholmod_rhs); + + const time_t solve_time = time(NULL); 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; @@ -277,9 +325,63 @@ bool SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { VectorRef(solution, num_rows) = VectorRef(static_cast<double*>(cholmod_solution->x), num_rows); ss_.Free(cholmod_solution); + const time_t final_time = time(NULL); + VLOG(2) << "time: " << (final_time - start_time) + << " lhs : " << (lhs_time - start_time) + << " rhs: " << (rhs_time - lhs_time) + << " analyze: " << (symbolic_time - rhs_time) + << " factor_and_solve: " << (solve_time - symbolic_time) + << " cleanup: " << (final_time - solve_time); return true; } +#else +bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( + double* solution) { + LOG(FATAL) << "No SuiteSparse support in Ceres."; + return false; +} #endif // CERES_NO_SUITESPARSE +#ifndef CERES_NO_CXSPARSE +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessSparseMatrix. The linear system is solved using +// CXSparse's sparse cholesky factorization routines. +bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse( + 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; + } + + cs_di* lhs = CHECK_NOTNULL(cxsparse_.CreateSparseMatrix(tsm)); + VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows); + + // Compute symbolic factorization if not available. + if (cxsparse_factor_ == NULL) { + cxsparse_factor_ = CHECK_NOTNULL(cxsparse_.AnalyzeCholesky(lhs)); + } + + // Solve the linear system. + bool ok = cxsparse_.SolveCholesky(lhs, cxsparse_factor_, solution); + + cxsparse_.Free(lhs); + return ok; +} +#else +bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse( + double* solution) { + LOG(FATAL) << "No CXSparse support in Ceres."; + return false; +} +#endif // CERES_NO_CXPARSE + } // 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 index 039bc09e3ce..ea1b3184c33 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h @@ -31,9 +31,13 @@ #ifndef CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ #define CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ +#include <set> +#include <utility> + #include "ceres/block_random_access_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" +#include "ceres/cxsparse.h" #include "ceres/linear_solver.h" #include "ceres/schur_eliminator.h" #include "ceres/suitesparse.h" @@ -128,7 +132,7 @@ class SchurComplementSolver : public BlockSparseMatrixBaseSolver { scoped_ptr<BlockRandomAccessMatrix> lhs_; scoped_array<double> rhs_; - DISALLOW_COPY_AND_ASSIGN(SchurComplementSolver); + CERES_DISALLOW_COPY_AND_ASSIGN(SchurComplementSolver); }; // Dense Cholesky factorization based solver. @@ -142,10 +146,10 @@ class DenseSchurComplementSolver : public SchurComplementSolver { virtual void InitStorage(const CompressedRowBlockStructure* bs); virtual bool SolveReducedLinearSystem(double* solution); - DISALLOW_COPY_AND_ASSIGN(DenseSchurComplementSolver); + CERES_DISALLOW_COPY_AND_ASSIGN(DenseSchurComplementSolver); }; -#ifndef CERES_NO_SUITESPARSE + // Sparse Cholesky factorization based solver. class SparseSchurComplementSolver : public SchurComplementSolver { public: @@ -155,26 +159,26 @@ class SparseSchurComplementSolver : public SchurComplementSolver { private: virtual void InitStorage(const CompressedRowBlockStructure* bs); virtual bool SolveReducedLinearSystem(double* solution); + bool SolveReducedLinearSystemUsingSuiteSparse(double* solution); + bool SolveReducedLinearSystemUsingCXSparse(double* solution); + // Size of the blocks in the Schur complement. + vector<int> blocks_; +#ifndef CERES_NO_SUITESPARSE 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."; - } + // once and reused in subsequent calls. + cholmod_factor* factor_; +#endif // CERES_NO_SUITESPARSE - virtual ~SparseSchurComplementSolver() {} +#ifndef CERES_NO_CXSPARSE + CXSparse cxsparse_; + // Cached factorization + cs_dis* cxsparse_factor_; +#endif // CERES_NO_CXSPARSE + CERES_DISALLOW_COPY_AND_ASSIGN(SparseSchurComplementSolver); }; -#endif // CERES_NO_SUITESPARSE } // namespace internal } // namespace ceres 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 index a388d005424..6120db9b009 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h @@ -188,7 +188,7 @@ Eliminate(const BlockSparseMatrixBase* A, typename EigenTypes<kFBlockSize>::ConstVectorRef diag(D + bs->cols[i].position, block_size); - MutexLock l(&cell_info->m); + CeresMutexLock 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(); @@ -387,7 +387,7 @@ UpdateRhs(const Chunk& chunk, row.block.size, block_size); const int block = block_id - num_eliminate_blocks_; - MutexLock l(rhs_locks_[block]); + CeresMutexLock l(rhs_locks_[block]); typename EigenTypes<kFBlockSize>::VectorRef (rhs + lhs_row_layout_[block], block_size).noalias() += b.transpose() * sj; @@ -523,7 +523,7 @@ ChunkOuterProduct(const CompressedRowBlockStructure* bs, const typename EigenTypes<kEBlockSize, kFBlockSize>::ConstMatrixRef b2(buffer + it2->second, e_block_size, block2_size); - MutexLock l(&cell_info->m); + CeresMutexLock l(&cell_info->m); MatrixRef m(cell_info->values, row_stride, col_stride); // We explicitly construct a block object here instead of using @@ -532,7 +532,29 @@ ChunkOuterProduct(const CompressedRowBlockStructure* bs, // 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; +#ifdef CERES_WORK_AROUND_ANDROID_NDK_COMPILER_BUG + // Removing the ".noalias()" annotation on the following statement is + // necessary to produce a correct build with the Android NDK, including + // versions 6, 7, 8, and 8b, when built with STLPort and the + // non-standalone toolchain (i.e. ndk-build). This appears to be a + // compiler bug; if the workaround is not in place, the line + // + // block.noalias() -= b1_transpose_inverse_ete * b2; + // + // gets compiled to + // + // block.noalias() += b1_transpose_inverse_ete * b2; + // + // which breaks schur elimination. Introducing a temporary by removing the + // .noalias() annotation causes the issue to disappear. Tracking this + // issue down was tricky, since the test suite doesn't run when built with + // the non-standalone toolchain. + // + // TODO(keir): Make a reproduction case for this and send it upstream. + block -= b1_transpose_inverse_ete * b2; +#else + block.noalias() -= b1_transpose_inverse_ete * b2; +#endif // CERES_WORK_AROUND_ANDROID_NDK_COMPILER_BUG } } } @@ -601,7 +623,7 @@ NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, &r, &c, &row_stride, &col_stride); if (cell_info != NULL) { - MutexLock l(&cell_info->m); + CeresMutexLock l(&cell_info->m); MatrixRef m(cell_info->values, row_stride, col_stride); m.block(r, c, block1_size, block1_size) .selfadjointView<Eigen::Upper>() @@ -621,7 +643,7 @@ NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, } const int block2_size = bs->cols[row.cells[j].block_id].size; - MutexLock l(&cell_info->m); + CeresMutexLock 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, @@ -660,7 +682,7 @@ EBlockRowOuterProduct(const BlockSparseMatrixBase* A, continue; } - MutexLock l(&cell_info->m); + CeresMutexLock l(&cell_info->m); MatrixRef m(cell_info->values, row_stride, col_stride); Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize> @@ -687,7 +709,7 @@ EBlockRowOuterProduct(const BlockSparseMatrixBase* A, row.block.size, block2_size); - MutexLock l(&cell_info->m); + CeresMutexLock 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); diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc index c4fc1da3c2f..1cdff4e6dec 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc @@ -30,25 +30,14 @@ #include "ceres/schur_ordering.h" -#include <glog/logging.h> #include "ceres/graph.h" #include "ceres/graph_algorithms.h" +#include "ceres/internal/scoped_ptr.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 +#include "glog/logging.h" namespace ceres { namespace internal { @@ -59,8 +48,7 @@ int ComputeSchurOrdering(const Program& program, scoped_ptr<Graph< ParameterBlock*> > graph( CHECK_NOTNULL(CreateHessianGraph(program))); - int independent_set_size = - IndependentSetOrdering<ParameterBlock*>(*graph, ordering); + int independent_set_size = IndependentSetOrdering(*graph, ordering); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); // Add the excluded blocks to back of the ordering vector. diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver.cc b/extern/libmv/third_party/ceres/internal/ceres/solver.cc index 77f04d1d918..66ca93283a1 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/solver.cc @@ -32,36 +32,32 @@ #include "ceres/solver.h" #include <vector> -#include "ceres/levenberg_marquardt.h" +#include "ceres/problem.h" +#include "ceres/problem_impl.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. +// TODO(sameeragarwal): Use subsecond timers. 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); + internal::ProblemImpl* problem_impl = + CHECK_NOTNULL(problem)->problem_impl_.get(); + internal::SolverImpl::Solve(options, problem_impl, 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 solver; + solver.Solve(options, problem, summary); } Solver::Summary::Summary() @@ -75,6 +71,7 @@ Solver::Summary::Summary() num_unsuccessful_steps(-1), preprocessor_time_in_seconds(-1.0), minimizer_time_in_seconds(-1.0), + postprocessor_time_in_seconds(-1.0), total_time_in_seconds(-1.0), num_parameter_blocks(-1), num_parameters(-1), @@ -93,7 +90,9 @@ Solver::Summary::Summary() linear_solver_type_given(SPARSE_NORMAL_CHOLESKY), linear_solver_type_used(SPARSE_NORMAL_CHOLESKY), preconditioner_type(IDENTITY), - ordering_type(NATURAL) { + ordering_type(NATURAL), + trust_region_strategy_type(LEVENBERG_MARQUARDT), + sparse_linear_algebra_library(SUITE_SPARSE) { } string Solver::Summary::BriefReport() const { @@ -136,7 +135,7 @@ string Solver::Summary::FullReport() const { num_parameters); internal::StringAppendF(&report, "Residual blocks % 10d\n", num_residual_blocks); - internal::StringAppendF(&report, "Residual % 10d\n\n", + internal::StringAppendF(&report, "Residuals % 10d\n\n", num_residuals); } else { internal::StringAppendF(&report, "%45s %21s\n", "Original", "Reduced"); @@ -183,10 +182,33 @@ string Solver::Summary::FullReport() const { internal::StringAppendF(&report, "Threads: % 25d% 25d\n", num_threads_given, num_threads_used); - internal::StringAppendF(&report, "Linear Solver Threads:% 23d% 25d\n", + internal::StringAppendF(&report, "Linear solver threads % 23d% 25d\n", num_linear_solver_threads_given, num_linear_solver_threads_used); + if (linear_solver_type_used == SPARSE_NORMAL_CHOLESKY || + linear_solver_type_used == SPARSE_SCHUR || + (linear_solver_type_used == ITERATIVE_SCHUR && + (preconditioner_type == SCHUR_JACOBI || + preconditioner_type == CLUSTER_JACOBI || + preconditioner_type == CLUSTER_TRIDIAGONAL))) { + internal::StringAppendF(&report, "\nSparse Linear Algebra Library %15s\n", + SparseLinearAlgebraLibraryTypeToString( + sparse_linear_algebra_library)); + } + + internal::StringAppendF(&report, "Trust Region Strategy %19s", + TrustRegionStrategyTypeToString( + trust_region_strategy_type)); + if (trust_region_strategy_type == DOGLEG) { + if (dogleg_type == TRADITIONAL_DOGLEG) { + internal::StringAppendF(&report, " (TRADITIONAL)"); + } else { + internal::StringAppendF(&report, " (SUBSPACE)"); + } + } + internal::StringAppendF(&report, "\n"); + if (termination_type == DID_NOT_RUN) { CHECK(!error.empty()) diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc index ed07d9dc6d7..2802a75effe 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc @@ -30,40 +30,29 @@ #include "ceres/solver_impl.h" +#include <cstdio> #include <iostream> // NOLINT #include <numeric> #include "ceres/evaluator.h" #include "ceres/gradient_checking_cost_function.h" -#include "ceres/levenberg_marquardt.h" +#include "ceres/iteration_callback.h" +#include "ceres/levenberg_marquardt_strategy.h" #include "ceres/linear_solver.h" #include "ceres/map_util.h" #include "ceres/minimizer.h" #include "ceres/parameter_block.h" +#include "ceres/problem.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" +#include "ceres/trust_region_minimizer.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 { @@ -96,7 +85,7 @@ class LoggingCallback : public IterationCallback { 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"; + "rho:% 3.2e mu:% 3.2e li:% 3d it:% 3.2e tt:% 3.2e"; string output = StringPrintf(kReportRowFormat, summary.iteration, summary.cost, @@ -104,8 +93,10 @@ class LoggingCallback : public IterationCallback { summary.gradient_max_norm, summary.step_norm, summary.relative_decrease, - summary.mu, - summary.linear_solver_iterations); + summary.trust_region_radius, + summary.linear_solver_iterations, + summary.iteration_time_in_seconds, + summary.cumulative_time_in_seconds); if (log_to_stdout_) { cout << output << endl; } else { @@ -118,44 +109,101 @@ class LoggingCallback : public IterationCallback { const bool log_to_stdout_; }; +// Basic callback to record the execution of the solver to a file for +// offline analysis. +class FileLoggingCallback : public IterationCallback { + public: + explicit FileLoggingCallback(const string& filename) + : fptr_(NULL) { + fptr_ = fopen(filename.c_str(), "w"); + CHECK_NOTNULL(fptr_); + } + + virtual ~FileLoggingCallback() { + if (fptr_ != NULL) { + fclose(fptr_); + } + } + + virtual CallbackReturnType operator()(const IterationSummary& summary) { + fprintf(fptr_, + "%4d %e %e\n", + summary.iteration, + summary.cost, + summary.cumulative_time_in_seconds); + return SOLVER_CONTINUE; + } + private: + FILE* fptr_; +}; + } // namespace void SolverImpl::Minimize(const Solver::Options& options, Program* program, Evaluator* evaluator, LinearSolver* linear_solver, - double* initial_parameters, - double* final_parameters, + double* parameters, Solver::Summary* summary) { Minimizer::Options minimizer_options(options); + // TODO(sameeragarwal): Add support for logging the configuration + // and more detailed stats. + scoped_ptr<IterationCallback> file_logging_callback; + if (!options.solver_log.empty()) { + file_logging_callback.reset(new FileLoggingCallback(options.solver_log)); + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + file_logging_callback.get()); + } + LoggingCallback logging_callback(options.minimizer_progress_to_stdout); if (options.logging_type != SILENT) { - minimizer_options.callbacks.push_back(&logging_callback); + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + &logging_callback); } - StateUpdatingCallback updating_callback(program, initial_parameters); + StateUpdatingCallback updating_callback(program, 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; + // This must get pushed to the front of the callbacks so that it is run + // before any of the user callbacks. + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + &updating_callback); + } + + minimizer_options.evaluator = evaluator; + scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); + minimizer_options.jacobian = jacobian.get(); + + TrustRegionStrategy::Options trust_region_strategy_options; + trust_region_strategy_options.linear_solver = linear_solver; + trust_region_strategy_options.initial_radius = + options.initial_trust_region_radius; + trust_region_strategy_options.max_radius = options.max_trust_region_radius; + trust_region_strategy_options.lm_min_diagonal = options.lm_min_diagonal; + trust_region_strategy_options.lm_max_diagonal = options.lm_max_diagonal; + trust_region_strategy_options.trust_region_strategy_type = + options.trust_region_strategy_type; + trust_region_strategy_options.dogleg_type = options.dogleg_type; + scoped_ptr<TrustRegionStrategy> strategy( + TrustRegionStrategy::Create(trust_region_strategy_options)); + minimizer_options.trust_region_strategy = strategy.get(); + + TrustRegionMinimizer minimizer; + time_t minimizer_start_time = time(NULL); + minimizer.Minimize(minimizer_options, parameters, summary); + summary->minimizer_time_in_seconds = time(NULL) - minimizer_start_time; } void SolverImpl::Solve(const Solver::Options& original_options, - Problem* problem, + ProblemImpl* original_problem_impl, Solver::Summary* summary) { + time_t solver_start_time = time(NULL); Solver::Options options(original_options); + Program* original_program = original_problem_impl->mutable_program(); + ProblemImpl* problem_impl = original_problem_impl; + // Reset the summary object to its default values. + *CHECK_NOTNULL(summary) = Solver::Summary(); + #ifndef CERES_USE_OPENMP if (options.num_threads > 1) { @@ -174,8 +222,6 @@ void SolverImpl::Solve(const Solver::Options& original_options, } #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; @@ -183,32 +229,38 @@ void SolverImpl::Solve(const Solver::Options& original_options, 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); + summary->sparse_linear_algebra_library = + options.sparse_linear_algebra_library; + summary->trust_region_strategy_type = options.trust_region_strategy_type; + summary->dogleg_type = options.dogleg_type; + + // Evaluate the initial cost, residual vector and the jacobian + // matrix if requested by the user. 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. + Evaluator::Evaluate( + original_program, + options.num_threads, + &(summary->initial_cost), + options.return_initial_residuals ? &summary->initial_residuals : NULL, + options.return_initial_gradient ? &summary->initial_gradient : NULL, + options.return_initial_jacobian ? &summary->initial_jacobian : NULL); + original_program->SetParameterBlockStatePtrsToUserStatePtrs(); // 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; + // Save the original problem impl so we don't use the gradient + // checking one when computing the residuals. if (options.check_gradients) { VLOG(1) << "Checking Gradients"; gradient_checking_problem_impl.reset( @@ -224,8 +276,10 @@ void SolverImpl::Solve(const Solver::Options& original_options, // 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)); + scoped_ptr<Program> reduced_program(CreateReducedProgram(&options, + problem_impl, + &summary->fixed_cost, + &summary->error)); if (reduced_program == NULL) { return; } @@ -259,19 +313,21 @@ void SolverImpl::Solve(const Solver::Options& original_options, } // The optimizer works on contiguous parameter vectors; allocate some. - Vector initial_parameters(reduced_program->NumParameters()); - Vector optimized_parameters(reduced_program->NumParameters()); + Vector parameters(reduced_program->NumParameters()); // Collect the discontiguous parameters into a contiguous state vector. - reduced_program->ParameterBlocksToStateVector(&initial_parameters[0]); + reduced_program->ParameterBlocksToStateVector(parameters.data()); + + time_t minimizer_start_time = time(NULL); + summary->preprocessor_time_in_seconds = + minimizer_start_time - solver_start_time; // Run the optimization. Minimize(options, reduced_program.get(), evaluator.get(), linear_solver.get(), - initial_parameters.data(), - optimized_parameters.data(), + parameters.data(), summary); // If the user aborted mid-optimization or the optimization @@ -282,30 +338,45 @@ void SolverImpl::Solve(const Solver::Options& original_options, return; } + time_t post_process_start_time = time(NULL); + // Push the contiguous optimized parameters back to the user's parameters. - reduced_program->StateVectorToParameterBlocks(&optimized_parameters[0]); + reduced_program->StateVectorToParameterBlocks(parameters.data()); 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); - + // Evaluate the final cost, residual vector and the jacobian + // matrix if requested by the user. + Evaluator::Evaluate( + original_program, + options.num_threads, + &summary->final_cost, + options.return_final_residuals ? &summary->final_residuals : NULL, + options.return_final_gradient ? &summary->final_gradient : NULL, + options.return_final_jacobian ? &summary->final_jacobian : NULL); + + // Ensure the program state is set to the user parameters on the way out. + original_program->SetParameterBlockStatePtrsToUserStatePtrs(); // Stick a fork in it, we're done. - return; + summary->postprocessor_time_in_seconds = time(NULL) - post_process_start_time; } // Strips varying parameters and residuals, maintaining order, and updating // num_eliminate_blocks. bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program, int* num_eliminate_blocks, + double* fixed_cost, string* error) { int original_num_eliminate_blocks = *num_eliminate_blocks; vector<ParameterBlock*>* parameter_blocks = program->mutable_parameter_blocks(); + scoped_array<double> residual_block_evaluate_scratch; + if (fixed_cost != NULL) { + residual_block_evaluate_scratch.reset( + new double[program->MaxScratchDoublesNeededForEvaluate()]); + *fixed_cost = 0.0; + } + // 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) { @@ -335,6 +406,17 @@ bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program, if (!all_constant) { (*residual_blocks)[j++] = (*residual_blocks)[i]; + } else if (fixed_cost != NULL) { + // The residual is constant and will be removed, so its cost is + // added to the variable fixed_cost. + double cost = 0.0; + if (!residual_block->Evaluate( + &cost, NULL, NULL, residual_block_evaluate_scratch.get())) { + *error = StringPrintf("Evaluation of the residual %d failed during " + "removal of fixed residual blocks.", i); + return false; + } + *fixed_cost += cost; } } residual_blocks->resize(j); @@ -367,6 +449,7 @@ bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program, Program* SolverImpl::CreateReducedProgram(Solver::Options* options, ProblemImpl* problem_impl, + double* fixed_cost, string* error) { Program* original_program = problem_impl->mutable_program(); scoped_ptr<Program> transformed_program(new Program(*original_program)); @@ -397,6 +480,7 @@ Program* SolverImpl::CreateReducedProgram(Solver::Options* options, if (!RemoveFixedBlocksFromProgram(transformed_program.get(), &num_eliminate_blocks, + fixed_cost, error)) { return NULL; } @@ -431,13 +515,34 @@ Program* SolverImpl::CreateReducedProgram(Solver::Options* options, LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, string* error) { + if (options->trust_region_strategy_type == DOGLEG) { + if (options->linear_solver_type == ITERATIVE_SCHUR || + options->linear_solver_type == CGNR) { + *error = "DOGLEG only supports exact factorization based linear " + "solvers. If you want to use an iterative solver please " + "use LEVENBERG_MARQUARDT as the trust_region_strategy_type"; + return NULL; + } + } + #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."; + if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && + options->sparse_linear_algebra_library == SUITE_SPARSE) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITESPARSE because " + "SuiteSparse was not enabled when Ceres was built."; + return NULL; + } +#endif + +#ifdef CERES_NO_CXSPARSE + if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && + options->sparse_linear_algebra_library == CX_SPARSE) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY with CXSPARSE because " + "CXSparse was not enabled when Ceres was built."; return NULL; } -#endif // CERES_NO_SUITESPARSE +#endif + if (options->linear_solver_max_num_iterations <= 0) { *error = "Solver::Options::linear_solver_max_num_iterations is 0."; @@ -455,30 +560,32 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, } 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; + linear_solver_options.sparse_linear_algebra_library = + options->sparse_linear_algebra_library; + linear_solver_options.use_block_amd = options->use_block_amd; #ifdef CERES_NO_SUITESPARSE if (linear_solver_options.preconditioner_type == SCHUR_JACOBI) { *error = "SCHUR_JACOBI preconditioner not suppored. Please build Ceres " - "with SuiteSparse support"; + "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"; + "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"; + "Ceres with SuiteSparse support."; return NULL; } #endif @@ -489,23 +596,23 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, if ((linear_solver_options.num_eliminate_blocks == 0) && IsSchurType(linear_solver_options.type)) { -#ifndef CERES_NO_SUITESPARSE +#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) + LOG(INFO) << "No elimination block remaining switching to DENSE_QR."; + linear_solver_options.type = DENSE_QR; +#else 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 +#endif } -#ifdef CERES_NO_SUITESPARSE +#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) if (linear_solver_options.type == SPARSE_SCHUR) { - *error = "Can't use SPARSE_SCHUR because SuiteSparse was not " - "enabled when Ceres was built."; + *error = "Can't use SPARSE_SCHUR because neither SuiteSparse nor" + "CXSparse was enabled when Ceres was compiled."; return NULL; } -#endif // CERES_NO_SUITESPARSE +#endif // The matrix used for storing the dense Schur complement has a // single lock guarding the whole matrix. Running the @@ -583,10 +690,13 @@ int MinParameterBlock(const ResidualBlock* residual_block, 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); + if (!parameter_block->IsConstant()) { + CHECK_NE(parameter_block->index(), -1) + << "Did you forget to call Program::SetParameterOffsetsAndIndex()? " + << "This is a Ceres bug; please contact the developers!"; + min_parameter_block_position = std::min(parameter_block->index(), + min_parameter_block_position); + } } return min_parameter_block_position; } diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h index 957ebcc65df..11b44de6f42 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h @@ -31,6 +31,9 @@ #ifndef CERES_INTERNAL_SOLVER_IMPL_H_ #define CERES_INTERNAL_SOLVER_IMPL_H_ +#include <string> +#include <vector> +#include "ceres/internal/port.h" #include "ceres/solver.h" namespace ceres { @@ -46,15 +49,18 @@ class SolverImpl { // Mirrors the interface in solver.h, but exposes implementation // details for testing internally. static void Solve(const Solver::Options& options, - Problem* problem, + ProblemImpl* problem_impl, 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. + // If fixed_cost is not NULL, the residual blocks that are removed + // are evaluated and the sum of their cost is returned in fixed_cost. static Program* CreateReducedProgram(Solver::Options* options, ProblemImpl* problem_impl, + double* fixed_cost, string* error); // Create the appropriate linear solver, taking into account any @@ -92,16 +98,18 @@ class SolverImpl { Program* program, Evaluator* evaluator, LinearSolver* linear_solver, - double* initial_parameters, - double* final_parameters, + double* 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. + // If fixed_cost is not NULL, the residual blocks that are removed + // are evaluated and the sum of their cost is returned in fixed_cost. static bool RemoveFixedBlocksFromProgram(Program* program, int* num_eliminate_blocks, + double* fixed_cost, string* error); }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h index 562210dfec8..1b19f887946 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h @@ -86,7 +86,7 @@ class SparseMatrix : public LinearOperator { // sparse matrix. virtual void ToDenseMatrix(Matrix* dense_matrix) const = 0; -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS // Dump the sparse matrix to a proto. Destroys the contents of proto. virtual void ToProto(SparseMatrixProto* proto) const = 0; #endif 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 index 59222dc374d..9e00b4402dc 100644 --- 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 @@ -28,13 +28,16 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) -#ifndef CERES_NO_SUITESPARSE - #include "ceres/sparse_normal_cholesky_solver.h" #include <algorithm> #include <cstring> #include <ctime> + +#ifndef CERES_NO_CXSPARSE +#include "cs.h" +#endif + #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/linear_solver.h" #include "ceres/suitesparse.h" @@ -48,13 +51,30 @@ namespace internal { SparseNormalCholeskySolver::SparseNormalCholeskySolver( const LinearSolver::Options& options) - : options_(options), symbolic_factor_(NULL) {} + : options_(options) { +#ifndef CERES_NO_SUITESPARSE + factor_ = NULL; +#endif + +#ifndef CERES_NO_CXSPARSE + cxsparse_factor_ = NULL; +#endif // CERES_NO_CXSPARSE +} SparseNormalCholeskySolver::~SparseNormalCholeskySolver() { - if (symbolic_factor_ != NULL) { - ss_.Free(symbolic_factor_); - symbolic_factor_ = NULL; +#ifndef CERES_NO_SUITESPARSE + if (factor_ != NULL) { + ss_.Free(factor_); + factor_ = NULL; } +#endif + +#ifndef CERES_NO_CXSPARSE + if (cxsparse_factor_ != NULL) { + cxsparse_.Free(cxsparse_factor_); + cxsparse_factor_ = NULL; + } +#endif // CERES_NO_CXSPARSE } LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( @@ -62,6 +82,93 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double * x) { + switch (options_.sparse_linear_algebra_library) { + case SUITE_SPARSE: + return SolveImplUsingSuiteSparse(A, b, per_solve_options, x); + case CX_SPARSE: + return SolveImplUsingCXSparse(A, b, per_solve_options, x); + default: + LOG(FATAL) << "Unknown sparse linear algebra library : " + << options_.sparse_linear_algebra_library; + } + + LOG(FATAL) << "Unknown sparse linear algebra library : " + << options_.sparse_linear_algebra_library; + return LinearSolver::Summary(); +} + +#ifndef CERES_NO_CXSPARSE +LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double * x) { + LinearSolver::Summary summary; + summary.num_iterations = 1; + const int num_cols = A->num_cols(); + 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(); + + // Wrap the augmented Jacobian in a compressed sparse column matrix. + cs_di At = cxsparse_.CreateSparseMatrixTransposeView(A); + + // Compute the normal equations. J'J delta = J'f and solve them + // using a sparse Cholesky factorization. Notice that when compared + // to SuiteSparse we have to explicitly compute the transpose of Jt, + // and then the normal equations before they can be + // factorized. CHOLMOD/SuiteSparse on the other hand can just work + // off of Jt to compute the Cholesky factorization of the normal + // equations. + cs_di* A2 = cs_transpose(&At, 1); + cs_di* AtA = cs_multiply(&At,A2); + + cxsparse_.Free(A2); + if (per_solve_options.D != NULL) { + A->DeleteRows(num_cols); + } + + // Compute symbolic factorization if not available. + if (cxsparse_factor_ == NULL) { + cxsparse_factor_ = CHECK_NOTNULL(cxsparse_.AnalyzeCholesky(AtA)); + } + + // Solve the linear system. + if (cxsparse_.SolveCholesky(AtA, cxsparse_factor_, Atb.data())) { + VectorRef(x, Atb.rows()) = Atb; + summary.termination_type = TOLERANCE; + } + + cxsparse_.Free(AtA); + return summary; +} +#else +LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double * x) { + LOG(FATAL) << "No CXSparse support in Ceres."; + + // Unreachable but MSVC does not know this. + return LinearSolver::Summary(); +} +#endif + +#ifndef CERES_NO_SUITESPARSE +LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( + 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(); @@ -84,13 +191,25 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( 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())); + if (factor_ == NULL) { + if (options_.use_block_amd) { + factor_ = ss_.BlockAnalyzeCholesky(lhs.get(), + A->col_blocks(), + A->row_blocks()); + } else { + factor_ = ss_.AnalyzeCholesky(lhs.get()); + } + + if (VLOG_IS_ON(2)) { + cholmod_print_common("Symbolic Analysis", ss_.mutable_cc()); + } } + CHECK_NOTNULL(factor_); + const time_t symbolic_time = time(NULL); - cholmod_dense* sol = ss_.SolveCholesky(lhs.get(), symbolic_factor_, rhs); + cholmod_dense* sol = ss_.SolveCholesky(lhs.get(), factor_, rhs); const time_t solve_time = time(NULL); ss_.Free(rhs); @@ -100,11 +219,6 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( 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)); @@ -115,15 +229,25 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( } 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; + 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; } +#else +LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double * x) { + LOG(FATAL) << "No SuiteSparse support in Ceres."; + + // Unreachable but MSVC does not know this. + return LinearSolver::Summary(); +} +#endif } // 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 index ce1d6d285be..40d9e0a0327 100644 --- 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 @@ -34,13 +34,10 @@ #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/cxsparse.h" #include "ceres/linear_solver.h" -#include "ceres/suitesparse.h" #include "ceres/internal/macros.h" +#include "ceres/suitesparse.h" namespace ceres { namespace internal { @@ -61,17 +58,36 @@ class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { const LinearSolver::PerSolveOptions& options, double* x); - const LinearSolver::Options options_; + LinearSolver::Summary SolveImplUsingSuiteSparse( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& options, + double* x); + + // Crashes if CSparse is not installed. + LinearSolver::Summary SolveImplUsingCXSparse( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& options, + double* x); + +#ifndef CERES_NO_SUITESPARSE SuiteSparse ss_; + // Cached factorization + cholmod_factor* factor_; +#endif // CERES_NO_SUITESPARSE +#ifndef CERES_NO_CXSPARSE + CXSparse cxsparse_; // Cached factorization - cholmod_factor* symbolic_factor_; - DISALLOW_COPY_AND_ASSIGN(SparseNormalCholeskySolver); + cs_dis* cxsparse_factor_; +#endif // CERES_NO_CXSPARSE + + const LinearSolver::Options options_; + CERES_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.h b/extern/libmv/third_party/ceres/internal/ceres/split.h new file mode 100644 index 00000000000..ec579e974da --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/split.h @@ -0,0 +1,21 @@ +// Copyright 2011 Google Inc. All Rights Reserved. +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_SPLIT_H_ +#define VISION_OPTIMIZATION_LEAST_SQUARES_INTERNAL_SPLIT_H_ + +#include <string> +#include <vector> +#include "ceres/internal/port.h" + +namespace ceres { + +// Split a string using one or more character delimiters, presented as a +// nul-terminated c string. Append the components to 'result'. If there are +// consecutive delimiters, this function skips over all of them. +void SplitStringUsing(const string& full, const char* delim, + vector<string>* res); + +} // namespace ceres + +#endif // CERES_INTERNAL_SPLIT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h index 30b974e7ae5..f2f907ab32d 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h +++ b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h @@ -54,34 +54,34 @@ namespace internal { // 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) \ +#define CERES_PRINTF_ATTRIBUTE(string_index, first_to_check) \ __attribute__((__format__ (__printf__, string_index, first_to_check))) -#define SCANF_ATTRIBUTE(string_index, first_to_check) \ +#define CERES_SCANF_ATTRIBUTE(string_index, first_to_check) \ __attribute__((__format__ (__scanf__, string_index, first_to_check))) #else -#define PRINTF_ATTRIBUTE(string_index, first_to_check) +#define CERES_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); + CERES_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); + CERES_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); + CERES_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 +#undef CERES_PRINTF_ATTRIBUTE } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc index 1cf6a7496a7..cf3c48f84e6 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc @@ -29,15 +29,14 @@ // Author: sameeragarwal@google.com (Sameer Agarwal) #ifndef CERES_NO_SUITESPARSE - #include "ceres/suitesparse.h" +#include <vector> #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; @@ -111,6 +110,13 @@ cholmod_dense* SuiteSparse::CreateDenseVector(const double* x, } cholmod_factor* SuiteSparse::AnalyzeCholesky(cholmod_sparse* A) { + // Cholmod can try multiple re-ordering strategies to find a fill + // reducing ordering. Here we just tell it use AMD with automatic + // matrix dependence choice of supernodal versus simplicial + // factorization. + cc_.nmethods = 1; + cc_.method[0].ordering = CHOLMOD_AMD; + cc_.supernodal = CHOLMOD_AUTO; cholmod_factor* factor = cholmod_analyze(A, &cc_); CHECK_EQ(cc_.status, CHOLMOD_OK) << "Cholmod symbolic analysis failed " << cc_.status; @@ -118,6 +124,153 @@ cholmod_factor* SuiteSparse::AnalyzeCholesky(cholmod_sparse* A) { return factor; } +cholmod_factor* SuiteSparse::BlockAnalyzeCholesky( + cholmod_sparse* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks) { + vector<int> ordering; + if (!BlockAMDOrdering(A, row_blocks, col_blocks, &ordering)) { + return NULL; + } + return AnalyzeCholeskyWithUserOrdering(A, ordering); +} + +cholmod_factor* SuiteSparse::AnalyzeCholeskyWithUserOrdering(cholmod_sparse* A, + const vector<int>& ordering) { + CHECK_EQ(ordering.size(), A->nrow); + cc_.nmethods = 1 ; + cc_.method[0].ordering = CHOLMOD_GIVEN; + cholmod_factor* factor = + cholmod_analyze_p(A, const_cast<int*>(&ordering[0]), NULL, 0, &cc_); + CHECK_EQ(cc_.status, CHOLMOD_OK) + << "Cholmod symbolic analysis failed " << cc_.status; + CHECK_NOTNULL(factor); + return factor; +} + +bool SuiteSparse::BlockAMDOrdering(const cholmod_sparse* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks, + vector<int>* ordering) { + const int num_row_blocks = row_blocks.size(); + const int num_col_blocks = col_blocks.size(); + + // Arrays storing the compressed column structure of the matrix + // incoding the block sparsity of A. + vector<int> block_cols; + vector<int> block_rows; + + ScalarMatrixToBlockMatrix(A, + row_blocks, + col_blocks, + &block_rows, + &block_cols); + + cholmod_sparse_struct block_matrix; + block_matrix.nrow = num_row_blocks; + block_matrix.ncol = num_col_blocks; + block_matrix.nzmax = block_rows.size(); + block_matrix.p = reinterpret_cast<void*>(&block_cols[0]); + block_matrix.i = reinterpret_cast<void*>(&block_rows[0]); + block_matrix.x = NULL; + block_matrix.stype = A->stype; + block_matrix.itype = CHOLMOD_INT; + block_matrix.xtype = CHOLMOD_PATTERN; + block_matrix.dtype = CHOLMOD_DOUBLE; + block_matrix.sorted = 1; + block_matrix.packed = 1; + + vector<int> block_ordering(num_row_blocks); + if (!cholmod_amd(&block_matrix, NULL, 0, &block_ordering[0], &cc_)) { + return false; + } + + BlockOrderingToScalarOrdering(row_blocks, block_ordering, ordering); + return true; +} + +void SuiteSparse::ScalarMatrixToBlockMatrix(const cholmod_sparse* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks, + vector<int>* block_rows, + vector<int>* block_cols) { + CHECK_NOTNULL(block_rows)->clear(); + CHECK_NOTNULL(block_cols)->clear(); + const int num_row_blocks = row_blocks.size(); + const int num_col_blocks = col_blocks.size(); + + vector<int> row_block_starts(num_row_blocks); + for (int i = 0, cursor = 0; i < num_row_blocks; ++i) { + row_block_starts[i] = cursor; + cursor += row_blocks[i]; + } + + // The reinterpret_cast is needed here because CHOLMOD stores arrays + // as void*. + const int* scalar_cols = reinterpret_cast<const int*>(A->p); + const int* scalar_rows = reinterpret_cast<const int*>(A->i); + + // This loop extracts the block sparsity of the scalar sparse matrix + // A. It does so by iterating over the columns, but only considering + // the columns corresponding to the first element of each column + // block. Within each column, the inner loop iterates over the rows, + // and detects the presence of a row block by checking for the + // presence of a non-zero entry corresponding to its first element. + block_cols->push_back(0); + int c = 0; + for (int col_block = 0; col_block < num_col_blocks; ++col_block) { + int column_size = 0; + for (int idx = scalar_cols[c]; idx < scalar_cols[c + 1]; ++idx) { + vector<int>::const_iterator it = lower_bound(row_block_starts.begin(), + row_block_starts.end(), + scalar_rows[idx]); + // Since we are using lower_bound, it will return the row id + // where the row block starts. For everything but the first row + // of the block, where these values will be the same, we can + // skip, as we only need the first row to detect the presence of + // the block. + // + // For rows all but the first row in the last row block, + // lower_bound will return row_block_starts.end(), but those can + // be skipped like the rows in other row blocks too. + if (it == row_block_starts.end() || *it != scalar_rows[idx]) { + continue; + } + + block_rows->push_back(it - row_block_starts.begin()); + ++column_size; + } + block_cols->push_back(block_cols->back() + column_size); + c += col_blocks[col_block]; + } +} + +void SuiteSparse::BlockOrderingToScalarOrdering( + const vector<int>& blocks, + const vector<int>& block_ordering, + vector<int>* scalar_ordering) { + CHECK_EQ(blocks.size(), block_ordering.size()); + const int num_blocks = blocks.size(); + + // block_starts = [0, block1, block1 + block2 ..] + vector<int> block_starts(num_blocks); + for (int i = 0, cursor = 0; i < num_blocks ; ++i) { + block_starts[i] = cursor; + cursor += blocks[i]; + } + + scalar_ordering->resize(block_starts.back() + blocks.back()); + int cursor = 0; + for (int i = 0; i < num_blocks; ++i) { + const int block_id = block_ordering[i]; + const int block_size = blocks[block_id]; + int block_position = block_starts[block_id]; + for (int j = 0; j < block_size; ++j) { + (*scalar_ordering)[cursor++] = block_position++; + } + } +} + bool SuiteSparse::Cholesky(cholmod_sparse* A, cholmod_factor* L) { CHECK_NOTNULL(A); CHECK_NOTNULL(L); diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h index 091e67a69a9..eb691c0c0ed 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h @@ -37,6 +37,7 @@ #include <cstring> #include <string> +#include <vector> #include <glog/logging.h> #include "cholmod.h" @@ -105,12 +106,35 @@ class SuiteSparse { 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. + // Find an ordering of A or AA' (if A is unsymmetric) that minimizes + // the fill-in in the Cholesky factorization of the corresponding + // matrix. This is done by using the AMD algorithm. + // + // Using this ordering, the symbolic Cholesky factorization of A (or + // AA') is computed and returned. + // + // 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); + cholmod_factor* BlockAnalyzeCholesky(cholmod_sparse* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks); + + // If A is symmetric, then compute the symbolic Cholesky + // factorization of A(ordering, ordering). If A is unsymmetric, then + // compute the symbolic factorization of + // A(ordering,:) A(ordering,:)'. + // + // 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* AnalyzeCholeskyWithUserOrdering(cholmod_sparse* A, + const vector<int>& ordering); + // 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 @@ -129,6 +153,56 @@ class SuiteSparse { cholmod_factor* L, cholmod_dense* b); + // By virtue of the modeling layer in Ceres being block oriented, + // all the matrices used by Ceres are also block oriented. When + // doing sparse direct factorization of these matrices the + // fill-reducing ordering algorithms (in particular AMD) can either + // be run on the block or the scalar form of these matrices. The two + // SuiteSparse::AnalyzeCholesky methods allows the the client to + // compute the symbolic factorization of a matrix by either using + // AMD on the matrix or a user provided ordering of the rows. + // + // But since the underlying matrices are block oriented, it is worth + // running AMD on just the block structre of these matrices and then + // lifting these block orderings to a full scalar ordering. This + // preserves the block structure of the permuted matrix, and exposes + // more of the super-nodal structure of the matrix to the numerical + // factorization routines. + // + // Find the block oriented AMD ordering of a matrix A, whose row and + // column blocks are given by row_blocks, and col_blocks + // respectively. The matrix may or may not be symmetric. The entries + // of col_blocks do not need to sum to the number of columns in + // A. If this is the case, only the first sum(col_blocks) are used + // to compute the ordering. + bool BlockAMDOrdering(const cholmod_sparse* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks, + vector<int>* ordering); + + // Given a set of blocks and a permutation of these blocks, compute + // the corresponding "scalar" ordering, where the scalar ordering of + // size sum(blocks). + static void BlockOrderingToScalarOrdering(const vector<int>& blocks, + const vector<int>& block_ordering, + vector<int>* scalar_ordering); + + // Extract the block sparsity pattern of the scalar sparse matrix + // A and return it in compressed column form. The compressed column + // form is stored in two vectors block_rows, and block_cols, which + // correspond to the row and column arrays in a compressed column sparse + // matrix. + // + // If c_ij is the block in the matrix A corresponding to row block i + // and column block j, then it is expected that A contains at least + // one non-zero entry corresponding to the top left entry of c_ij, + // as that entry is used to detect the presence of a non-zero c_ij. + static void ScalarMatrixToBlockMatrix(const cholmod_sparse* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks, + vector<int>* block_rows, + vector<int>* block_cols); + 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_); } 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 index 247ab2e697b..ed8677ea18a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc @@ -32,12 +32,12 @@ #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/matrix_proto.h" #include "ceres/types.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -82,7 +82,7 @@ TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig) CopyData(orig); } -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS TripletSparseMatrix::TripletSparseMatrix(const SparseMatrixProto& outer_proto) { CHECK(outer_proto.has_triplet_matrix()); @@ -130,7 +130,7 @@ bool TripletSparseMatrix::AllTripletsWithinBounds() const { void TripletSparseMatrix::Reserve(int new_max_num_nonzeros) { CHECK_LE(num_nonzeros_, new_max_num_nonzeros) - << "Reallocation will cause data loss"; + << "Reallocation will cause data loss"; // Nothing to do if we have enough space already. if (new_max_num_nonzeros <= max_num_nonzeros_) @@ -214,7 +214,7 @@ void TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { } } -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS void TripletSparseMatrix::ToProto(SparseMatrixProto *proto) const { proto->Clear(); 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 index 300e74d0bbc..89a645bd879 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h @@ -50,7 +50,7 @@ class TripletSparseMatrix : public SparseMatrix { TripletSparseMatrix(); TripletSparseMatrix(int num_rows, int num_cols, int max_num_nonzeros); explicit TripletSparseMatrix(const TripletSparseMatrix& orig); -#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#ifndef CERES_NO_PROTOCOL_BUFFERS explicit TripletSparseMatrix(const SparseMatrixProto& proto); #endif @@ -65,7 +65,7 @@ class TripletSparseMatrix : public SparseMatrix { 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 +#ifndef CERES_NO_PROTOCOL_BUFFERS virtual void ToProto(SparseMatrixProto *proto) const; #endif virtual void ToTextFile(FILE* file) const; diff --git a/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.cc b/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.cc new file mode 100644 index 00000000000..76c4f8a7580 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.cc @@ -0,0 +1,550 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/trust_region_minimizer.h" + +#include <algorithm> +#include <cstdlib> +#include <cmath> +#include <cstring> +#include <limits> +#include <string> +#include <vector> + +#include "Eigen/Core" +#include "ceres/array_utils.h" +#include "ceres/evaluator.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_least_squares_problems.h" +#include "ceres/sparse_matrix.h" +#include "ceres/trust_region_strategy.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { +// Small constant for various floating point issues. +const double kEpsilon = 1e-12; +} // namespace + +// Execute the list of IterationCallbacks sequentially. If any one of +// the callbacks does not return SOLVER_CONTINUE, then stop and return +// its status. +CallbackReturnType TrustRegionMinimizer::RunCallbacks( + const IterationSummary& iteration_summary) { + for (int i = 0; i < options_.callbacks.size(); ++i) { + const CallbackReturnType status = + (*options_.callbacks[i])(iteration_summary); + if (status != SOLVER_CONTINUE) { + return status; + } + } + return SOLVER_CONTINUE; +} + +// Compute a scaling vector that is used to improve the conditioning +// of the Jacobian. +void TrustRegionMinimizer::EstimateScale(const SparseMatrix& jacobian, + double* scale) const { + jacobian.SquaredColumnNorm(scale); + for (int i = 0; i < jacobian.num_cols(); ++i) { + scale[i] = 1.0 / (kEpsilon + sqrt(scale[i])); + } +} + +void TrustRegionMinimizer::Init(const Minimizer::Options& options) { + options_ = options; + sort(options_.lsqp_iterations_to_dump.begin(), + options_.lsqp_iterations_to_dump.end()); +} + +bool TrustRegionMinimizer::MaybeDumpLinearLeastSquaresProblem( + const int iteration, + const SparseMatrix* jacobian, + const double* residuals, + const double* step) const { + // TODO(sameeragarwal): Since the use of trust_region_radius has + // moved inside TrustRegionStrategy, its not clear how we dump the + // regularization vector/matrix anymore. + // + // Doing this right requires either an API change to the + // TrustRegionStrategy and/or how LinearLeastSquares problems are + // stored on disk. + // + // For now, we will just not dump the regularizer. + return (!binary_search(options_.lsqp_iterations_to_dump.begin(), + options_.lsqp_iterations_to_dump.end(), + iteration) || + DumpLinearLeastSquaresProblem(options_.lsqp_dump_directory, + iteration, + options_.lsqp_dump_format_type, + jacobian, + NULL, + residuals, + step, + options_.num_eliminate_blocks)); +} + +void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary) { + time_t start_time = time(NULL); + time_t iteration_start_time = start_time; + Init(options); + + summary->termination_type = NO_CONVERGENCE; + summary->num_successful_steps = 0; + summary->num_unsuccessful_steps = 0; + + Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator); + SparseMatrix* jacobian = CHECK_NOTNULL(options_.jacobian); + TrustRegionStrategy* strategy = CHECK_NOTNULL(options_.trust_region_strategy); + + const int num_parameters = evaluator->NumParameters(); + const int num_effective_parameters = evaluator->NumEffectiveParameters(); + const int num_residuals = evaluator->NumResiduals(); + + VectorRef x_min(parameters, num_parameters); + Vector x = x_min; + double x_norm = x.norm(); + + Vector residuals(num_residuals); + Vector trust_region_step(num_effective_parameters); + Vector delta(num_effective_parameters); + Vector x_plus_delta(num_parameters); + Vector gradient(num_effective_parameters); + Vector model_residuals(num_residuals); + Vector scale(num_effective_parameters); + + IterationSummary iteration_summary; + iteration_summary.iteration = 0; + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; + iteration_summary.cost = summary->initial_cost; + iteration_summary.cost_change = 0.0; + iteration_summary.gradient_max_norm = 0.0; + iteration_summary.step_norm = 0.0; + iteration_summary.relative_decrease = 0.0; + iteration_summary.trust_region_radius = strategy->Radius(); + // TODO(sameeragarwal): Rename eta to linear_solver_accuracy or + // something similar across the board. + iteration_summary.eta = options_.eta; + iteration_summary.linear_solver_iterations = 0; + iteration_summary.step_solver_time_in_seconds = 0; + + // Do initial cost and Jacobian evaluation. + double cost = 0.0; + if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), NULL, jacobian)) { + LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + int num_consecutive_nonmonotonic_steps = 0; + double minimum_cost = cost; + double reference_cost = cost; + double accumulated_reference_model_cost_change = 0.0; + double candidate_cost = cost; + double accumulated_candidate_model_cost_change = 0.0; + + gradient.setZero(); + jacobian->LeftMultiply(residuals.data(), gradient.data()); + iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); + + if (options_.jacobi_scaling) { + EstimateScale(*jacobian, scale.data()); + jacobian->ScaleColumns(scale.data()); + } else { + scale.setOnes(); + } + + // The initial gradient max_norm is bounded from below so that we do + // not divide by zero. + const double gradient_max_norm_0 = + max(iteration_summary.gradient_max_norm, kEpsilon); + const double absolute_gradient_tolerance = + options_.gradient_tolerance * gradient_max_norm_0; + + if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating: Gradient tolerance reached." + << "Relative gradient max norm: " + << iteration_summary.gradient_max_norm / gradient_max_norm_0 + << " <= " << options_.gradient_tolerance; + return; + } + + iteration_summary.iteration_time_in_seconds = + time(NULL) - iteration_start_time; + iteration_summary.cumulative_time_in_seconds = time(NULL) - start_time + + summary->preprocessor_time_in_seconds; + summary->iterations.push_back(iteration_summary); + + // Call the various callbacks. + switch (RunCallbacks(iteration_summary)) { + case SOLVER_TERMINATE_SUCCESSFULLY: + summary->termination_type = USER_SUCCESS; + VLOG(1) << "Terminating: User callback returned USER_SUCCESS."; + return; + case SOLVER_ABORT: + summary->termination_type = USER_ABORT; + VLOG(1) << "Terminating: User callback returned USER_ABORT."; + return; + case SOLVER_CONTINUE: + break; + default: + LOG(FATAL) << "Unknown type of user callback status"; + } + + int num_consecutive_invalid_steps = 0; + while (true) { + iteration_start_time = time(NULL); + if (iteration_summary.iteration >= options_.max_num_iterations) { + summary->termination_type = NO_CONVERGENCE; + VLOG(1) << "Terminating: Maximum number of iterations reached."; + break; + } + + const double total_solver_time = iteration_start_time - start_time + + summary->preprocessor_time_in_seconds; + if (total_solver_time >= options_.max_solver_time_in_seconds) { + summary->termination_type = NO_CONVERGENCE; + VLOG(1) << "Terminating: Maximum solver time reached."; + break; + } + + iteration_summary = IterationSummary(); + iteration_summary = summary->iterations.back(); + iteration_summary.iteration = summary->iterations.back().iteration + 1; + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; + + const time_t strategy_start_time = time(NULL); + TrustRegionStrategy::PerSolveOptions per_solve_options; + per_solve_options.eta = options_.eta; + TrustRegionStrategy::Summary strategy_summary = + strategy->ComputeStep(per_solve_options, + jacobian, + residuals.data(), + trust_region_step.data()); + + iteration_summary.step_solver_time_in_seconds = + time(NULL) - strategy_start_time; + iteration_summary.linear_solver_iterations = + strategy_summary.num_iterations; + + if (!MaybeDumpLinearLeastSquaresProblem(iteration_summary.iteration, + jacobian, + residuals.data(), + trust_region_step.data())) { + LOG(FATAL) << "Tried writing linear least squares problem: " + << options.lsqp_dump_directory << "but failed."; + } + + double new_model_cost = 0.0; + if (strategy_summary.termination_type != FAILURE) { + // new_model_cost = 1/2 |f + J * step|^2 + model_residuals = residuals; + jacobian->RightMultiply(trust_region_step.data(), model_residuals.data()); + new_model_cost = model_residuals.squaredNorm() / 2.0; + + // In exact arithmetic, this would never be the case. But poorly + // conditioned matrices can give rise to situations where the + // new_model_cost can actually be larger than half the squared + // norm of the residual vector. We allow for small tolerance + // around cost and beyond that declare the step to be invalid. + if ((1.0 - new_model_cost / cost) < -kEpsilon) { + VLOG(1) << "Invalid step: current_cost: " << cost + << " new_model_cost " << new_model_cost + << " absolute difference " << (cost - new_model_cost) + << " relative difference " << (1.0 - new_model_cost/cost); + } else { + iteration_summary.step_is_valid = true; + } + } + + if (!iteration_summary.step_is_valid) { + // Invalid steps can happen due to a number of reasons, and we + // allow a limited number of successive failures, and return with + // NUMERICAL_FAILURE if this limit is exceeded. + if (++num_consecutive_invalid_steps >= + options_.max_num_consecutive_invalid_steps) { + summary->termination_type = NUMERICAL_FAILURE; + LOG(WARNING) << "Terminating. Number of successive invalid steps more " + << "than " + << "Solver::Options::max_num_consecutive_invalid_steps: " + << options_.max_num_consecutive_invalid_steps; + return; + } + + // We are going to try and reduce the trust region radius and + // solve again. To do this, we are going to treat this iteration + // as an unsuccessful iteration. Since the various callbacks are + // still executed, we are going to fill the iteration summary + // with data that assumes a step of length zero and no progress. + iteration_summary.cost = cost; + iteration_summary.cost_change = 0.0; + iteration_summary.gradient_max_norm = + summary->iterations.back().gradient_max_norm; + iteration_summary.step_norm = 0.0; + iteration_summary.relative_decrease = 0.0; + iteration_summary.eta = options_.eta; + } else { + // The step is numerically valid, so now we can judge its quality. + num_consecutive_invalid_steps = 0; + + // We allow some slop around 0, and clamp the model_cost_change + // at kEpsilon * min(1.0, cost) from below. + // + // In exact arithmetic this should never be needed, as we are + // guaranteed to new_model_cost <= cost. However, due to various + // numerical issues, it is possible that new_model_cost is + // nearly equal to cost, and the difference is a small negative + // number. To make sure that the relative_decrease computation + // remains sane, as clamp the difference (cost - new_model_cost) + // from below at a small positive number. + // + // This number is the minimum of kEpsilon * (cost, 1.0), which + // ensures that it will never get too large in absolute value, + // while scaling down proportionally with the magnitude of the + // cost. This is important for problems where the minimum of the + // objective function is near zero. + const double model_cost_change = + max(kEpsilon * min(1.0, cost), cost - new_model_cost); + + // Undo the Jacobian column scaling. + delta = (trust_region_step.array() * scale.array()).matrix(); + iteration_summary.step_norm = delta.norm(); + + // Convergence based on parameter_tolerance. + const double step_size_tolerance = options_.parameter_tolerance * + (x_norm + options_.parameter_tolerance); + if (iteration_summary.step_norm <= step_size_tolerance) { + VLOG(1) << "Terminating. Parameter tolerance reached. " + << "relative step_norm: " + << iteration_summary.step_norm / + (x_norm + options_.parameter_tolerance) + << " <= " << options_.parameter_tolerance; + summary->termination_type = PARAMETER_TOLERANCE; + return; + } + + if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) { + summary->termination_type = NUMERICAL_FAILURE; + LOG(WARNING) << "Terminating. Failed to compute " + << "Plus(x, delta, x_plus_delta)."; + return; + } + + // Try this step. + double new_cost; + if (!evaluator->Evaluate(x_plus_delta.data(), + &new_cost, + NULL, NULL, NULL)) { + // If the evaluation of the new cost fails, treat it as a step + // with high cost. + LOG(WARNING) << "Step failed to evaluate. " + << "Treating it as step with infinite cost"; + new_cost = numeric_limits<double>::max(); + } + + VLOG(2) << "old cost: " << cost << " new cost: " << new_cost; + iteration_summary.cost_change = cost - new_cost; + const double absolute_function_tolerance = + options_.function_tolerance * cost; + if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) { + VLOG(1) << "Terminating. Function tolerance reached. " + << "|cost_change|/cost: " + << fabs(iteration_summary.cost_change) / cost + << " <= " << options_.function_tolerance; + summary->termination_type = FUNCTION_TOLERANCE; + return; + } + + const double relative_decrease = + iteration_summary.cost_change / model_cost_change; + + const double historical_relative_decrease = + (reference_cost - new_cost) / + (accumulated_reference_model_cost_change + model_cost_change); + + // If monotonic steps are being used, then the relative_decrease + // is the usual ratio of the change in objective function value + // divided by the change in model cost. + // + // If non-monotonic steps are allowed, then we take the maximum + // of the relative_decrease and the + // historical_relative_decrease, which measures the increase + // from a reference iteration. The model cost change is + // estimated by accumulating the model cost changes since the + // reference iteration. The historical relative_decrease offers + // a boost to a step which is not too bad compared to the + // reference iteration, allowing for non-monotonic steps. + iteration_summary.relative_decrease = + options.use_nonmonotonic_steps + ? max(relative_decrease, historical_relative_decrease) + : relative_decrease; + + iteration_summary.step_is_successful = + iteration_summary.relative_decrease > options_.min_relative_decrease; + + if (iteration_summary.step_is_successful) { + accumulated_candidate_model_cost_change += model_cost_change; + accumulated_reference_model_cost_change += model_cost_change; + if (relative_decrease <= options_.min_relative_decrease) { + VLOG(2) << "Non-monotonic step! " + << " relative_decrease: " << relative_decrease + << " historical_relative_decrease: " + << historical_relative_decrease; + } + } + } + + if (iteration_summary.step_is_successful) { + ++summary->num_successful_steps; + strategy->StepAccepted(iteration_summary.relative_decrease); + x = x_plus_delta; + x_norm = x.norm(); + + // Step looks good, evaluate the residuals and Jacobian at this + // point. + if (!evaluator->Evaluate(x.data(), + &cost, + residuals.data(), + NULL, + jacobian)) { + summary->termination_type = NUMERICAL_FAILURE; + LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed."; + return; + } + + gradient.setZero(); + jacobian->LeftMultiply(residuals.data(), gradient.data()); + iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); + + if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating: Gradient tolerance reached." + << "Relative gradient max norm: " + << iteration_summary.gradient_max_norm / gradient_max_norm_0 + << " <= " << options_.gradient_tolerance; + return; + } + + if (options_.jacobi_scaling) { + jacobian->ScaleColumns(scale.data()); + } + + // Update the best, reference and candidate iterates. + // + // Based on algorithm 10.1.2 (page 357) of "Trust Region + // Methods" by Conn Gould & Toint, or equations 33-40 of + // "Non-monotone trust-region algorithms for nonlinear + // optimization subject to convex constraints" by Phil Toint, + // Mathematical Programming, 77, 1997. + if (cost < minimum_cost) { + // A step that improves solution quality was found. + x_min = x; + minimum_cost = cost; + // Set the candidate iterate to the current point. + candidate_cost = cost; + num_consecutive_nonmonotonic_steps = 0; + accumulated_candidate_model_cost_change = 0.0; + } else { + ++num_consecutive_nonmonotonic_steps; + if (cost > candidate_cost) { + // The current iterate is has a higher cost than the + // candidate iterate. Set the candidate to this point. + VLOG(2) << "Updating the candidate iterate to the current point."; + candidate_cost = cost; + accumulated_candidate_model_cost_change = 0.0; + } + + // At this point we have made too many non-monotonic steps and + // we are going to reset the value of the reference iterate so + // as to force the algorithm to descend. + // + // This is the case because the candidate iterate has a value + // greater than minimum_cost but smaller than the reference + // iterate. + if (num_consecutive_nonmonotonic_steps == + options.max_consecutive_nonmonotonic_steps) { + VLOG(2) << "Resetting the reference point to the candidate point"; + reference_cost = candidate_cost; + accumulated_reference_model_cost_change = + accumulated_candidate_model_cost_change; + } + } + } else { + ++summary->num_unsuccessful_steps; + if (iteration_summary.step_is_valid) { + strategy->StepRejected(iteration_summary.relative_decrease); + } else { + strategy->StepIsInvalid(); + } + } + + iteration_summary.cost = cost + summary->fixed_cost; + iteration_summary.trust_region_radius = strategy->Radius(); + if (iteration_summary.trust_region_radius < + options_.min_trust_region_radius) { + summary->termination_type = PARAMETER_TOLERANCE; + VLOG(1) << "Termination. Minimum trust region radius reached."; + return; + } + + iteration_summary.iteration_time_in_seconds = + time(NULL) - iteration_start_time; + iteration_summary.cumulative_time_in_seconds = time(NULL) - start_time + + summary->preprocessor_time_in_seconds; + summary->iterations.push_back(iteration_summary); + + switch (RunCallbacks(iteration_summary)) { + case SOLVER_TERMINATE_SUCCESSFULLY: + summary->termination_type = USER_SUCCESS; + VLOG(1) << "Terminating: User callback returned USER_SUCCESS."; + return; + case SOLVER_ABORT: + summary->termination_type = USER_ABORT; + VLOG(1) << "Terminating: User callback returned USER_ABORT."; + return; + case SOLVER_CONTINUE: + break; + default: + LOG(FATAL) << "Unknown type of user callback status"; + } + } +} + + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.h b/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.h new file mode 100644 index 00000000000..a4f5ba3674d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.h @@ -0,0 +1,67 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ +#define CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ + +#include "ceres/minimizer.h" +#include "ceres/solver.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// Generic trust region minimization algorithm. The heavy lifting is +// done by a TrustRegionStrategy object passed in as one of the +// arguments to the Minimize method. +// +// For example usage, see SolverImpl::Minimize. +class TrustRegionMinimizer : public Minimizer { + public: + ~TrustRegionMinimizer() {} + virtual void Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary); + + private: + void Init(const Minimizer::Options& options); + void EstimateScale(const SparseMatrix& jacobian, double* scale) const; + CallbackReturnType RunCallbacks(const IterationSummary& iteration_summary); + bool MaybeDumpLinearLeastSquaresProblem( const int iteration, + const SparseMatrix* jacobian, + const double* residuals, + const double* step) const; + + Minimizer::Options options_; +}; + +} // namespace internal +} // namespace ceres +#endif // CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.cc b/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.cc new file mode 100644 index 00000000000..89bc19d084b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.cc @@ -0,0 +1,27 @@ +#include "ceres/trust_region_strategy.h" +#include "ceres/dogleg_strategy.h" +#include "ceres/levenberg_marquardt_strategy.h" + +namespace ceres { +namespace internal { + +TrustRegionStrategy::~TrustRegionStrategy() {} + +TrustRegionStrategy* TrustRegionStrategy::Create(const Options& options) { + switch (options.trust_region_strategy_type) { + case LEVENBERG_MARQUARDT: + return new LevenbergMarquardtStrategy(options); + case DOGLEG: + return new DoglegStrategy(options); + default: + LOG(FATAL) << "Unknown trust region strategy: " + << options.trust_region_strategy_type; + } + + LOG(FATAL) << "Unknown trust region strategy: " + << options.trust_region_strategy_type; + return NULL; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.h b/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.h new file mode 100644 index 00000000000..391da97d5eb --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.h @@ -0,0 +1,148 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ +#define CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ + +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class LinearSolver; +class SparseMatrix; + +// Interface for classes implementing various trust region strategies +// for nonlinear least squares problems. +// +// The object is expected to maintain and update a trust region +// radius, which it then uses to solve for the trust region step using +// the jacobian matrix and residual vector. +// +// Here the term trust region radius is used loosely, as the strategy +// is free to treat it as guidance and violate it as need be. e.g., +// the LevenbergMarquardtStrategy uses the inverse of the trust region +// radius to scale the damping term, which controls the step size, but +// does not set a hard limit on its size. +class TrustRegionStrategy { +public: + struct Options { + Options() + : trust_region_strategy_type(LEVENBERG_MARQUARDT), + initial_radius(1e4), + max_radius(1e32), + lm_min_diagonal(1e-6), + lm_max_diagonal(1e32), + dogleg_type(TRADITIONAL_DOGLEG) { + } + + TrustRegionStrategyType trust_region_strategy_type; + // Linear solver used for actually solving the trust region step. + LinearSolver* linear_solver; + double initial_radius; + double max_radius; + + // Minimum and maximum values of the diagonal damping matrix used + // by LevenbergMarquardtStrategy. The DoglegStrategy also uses + // these bounds to construct a regularizing diagonal to ensure + // that the Gauss-Newton step computation is of full rank. + double lm_min_diagonal; + double lm_max_diagonal; + + // Further specify which dogleg method to use + DoglegType dogleg_type; + }; + + // Per solve options. + struct PerSolveOptions { + // Forcing sequence for inexact solves. + double eta; + }; + + struct Summary { + Summary() + : residual_norm(0.0), + num_iterations(-1), + termination_type(FAILURE) { + } + + // If the trust region problem is, + // + // 1/2 x'Ax + b'x + c, + // + // then + // + // residual_norm = |Ax -b| + double residual_norm; + + // Number of iterations used by the linear solver. If a linear + // solver was not called (e.g., DogLegStrategy after an + // unsuccessful step), then this would be zero. + int num_iterations; + + // Status of the linear solver used to solve the Newton system. + LinearSolverTerminationType termination_type; + }; + + virtual ~TrustRegionStrategy(); + + // Use the current radius to solve for the trust region step. + virtual Summary ComputeStep(const PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals, + double* step) = 0; + + // Inform the strategy that the current step has been accepted, and + // that the ratio of the decrease in the non-linear objective to the + // decrease in the trust region model is step_quality. + virtual void StepAccepted(double step_quality) = 0; + + // Inform the strategy that the current step has been rejected, and + // that the ratio of the decrease in the non-linear objective to the + // decrease in the trust region model is step_quality. + virtual void StepRejected(double step_quality) = 0; + + // Inform the strategy that the current step has been rejected + // because it was found to be numerically invalid. + // StepRejected/StepAccepted will not be called for this step, and + // the strategy is free to do what it wants with this information. + virtual void StepIsInvalid() = 0; + + // Current trust region radius. + virtual double Radius() const = 0; + + // Factory. + static TrustRegionStrategy* Create(const Options& options); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/types.cc b/extern/libmv/third_party/ceres/internal/ceres/types.cc index 860f8a43f37..2e950c51de7 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/types.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/types.cc @@ -37,8 +37,9 @@ namespace ceres { const char* LinearSolverTypeToString(LinearSolverType solver_type) { switch (solver_type) { - CASESTR(SPARSE_NORMAL_CHOLESKY); + CASESTR(DENSE_NORMAL_CHOLESKY); CASESTR(DENSE_QR); + CASESTR(SPARSE_NORMAL_CHOLESKY); CASESTR(DENSE_SCHUR); CASESTR(SPARSE_SCHUR); CASESTR(ITERATIVE_SCHUR); @@ -61,6 +62,16 @@ const char* PreconditionerTypeToString( } } +const char* SparseLinearAlgebraLibraryTypeToString( + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type) { + switch (sparse_linear_algebra_library_type) { + CASESTR(SUITE_SPARSE); + CASESTR(CX_SPARSE); + default: + return "UNKNOWN"; + } +} + const char* OrderingTypeToString(OrderingType ordering_type) { switch (ordering_type) { CASESTR(NATURAL); @@ -87,6 +98,26 @@ const char* SolverTerminationTypeToString( } } +const char* SparseLinearAlgebraTypeToString( + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type) { + switch (sparse_linear_algebra_library_type) { + CASESTR(CX_SPARSE); + CASESTR(SUITE_SPARSE); + default: + return "UNKNOWN"; + } +} + +const char* TrustRegionStrategyTypeToString( + TrustRegionStrategyType trust_region_strategy_type) { + switch (trust_region_strategy_type) { + CASESTR(LEVENBERG_MARQUARDT); + CASESTR(DOGLEG); + default: + return "UNKNOWN"; + } +} + #undef CASESTR bool IsSchurType(LinearSolverType type) { diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility.cc b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc index fd41648a7af..9d806541554 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/visibility.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc @@ -34,11 +34,10 @@ #include <set> #include <vector> #include <utility> - -#include <glog/logging.h> #include "ceres/block_structure.h" #include "ceres/collections_port.h" #include "ceres/graph.h" +#include "glog/logging.h" namespace ceres { namespace internal { 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 index aca77528215..4caad03d7a1 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc @@ -33,11 +33,9 @@ #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" @@ -46,10 +44,11 @@ #include "ceres/detect_structure.h" #include "ceres/graph.h" #include "ceres/graph_algorithms.h" +#include "ceres/internal/scoped_ptr.h" #include "ceres/linear_solver.h" #include "ceres/schur_eliminator.h" #include "ceres/visibility.h" -#include "ceres/internal/scoped_ptr.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -253,7 +252,6 @@ void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner( } 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; @@ -304,8 +302,6 @@ void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner( 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)); } } } @@ -322,17 +318,13 @@ void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner( 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"; + VLOG(1) << "Block pair stats: " << block_pairs_.size(); } // Initialize the SchurEliminator. @@ -341,7 +333,6 @@ void VisibilityBasedPreconditioner::InitEliminator( 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, @@ -352,9 +343,9 @@ void VisibilityBasedPreconditioner::InitEliminator( 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) { +// Update the values of the preconditioner matrix and factorize it. +bool VisibilityBasedPreconditioner::Update(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); @@ -448,12 +439,21 @@ bool VisibilityBasedPreconditioner::Factorize() { // matrix contains the values. lhs->stype = 1; - // Symbolic factorization is computed if we don't already have one - // handy. + // Symbolic factorization is computed if we don't already have one handy. if (factor_ == NULL) { - factor_ = ss_.AnalyzeCholesky(lhs); + if (options_.use_block_amd) { + factor_ = ss_.BlockAnalyzeCholesky(lhs, block_size_, block_size_); + } else { + factor_ = ss_.AnalyzeCholesky(lhs); + } + + if (VLOG_IS_ON(2)) { + cholmod_print_common("Symbolic Analysis", ss_.mutable_cc()); + } } + CHECK_NOTNULL(factor_); + bool status = ss_.Cholesky(lhs, factor_); ss_.Free(lhs); return status; 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 index fa095ca1dd8..888c65eba3a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h @@ -133,7 +133,7 @@ class SchurEliminatorBase; // options.num_eliminate_blocks = num_points; // VisibilityBasedPreconditioner preconditioner( // *A.block_structure(), options); -// preconditioner.Compute(A, NULL); +// preconditioner.Update(A, NULL); // preconditioner.RightMultiply(x, y); // @@ -160,7 +160,7 @@ class VisibilityBasedPreconditioner : public LinearOperator { const LinearSolver::Options& options); virtual ~VisibilityBasedPreconditioner(); - // Compute the numerical value of the preconditioner for the linear + // Update the numerical value of the preconditioner for the linear // system: // // | A | x = |b| @@ -171,12 +171,12 @@ class VisibilityBasedPreconditioner : public LinearOperator { // // D can be NULL, in which case its interpreted as a diagonal matrix // of size zero. - bool Compute(const BlockSparseMatrixBase& A, - const double* D); + bool Update(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 + // num_rows respectively. Update() 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 { @@ -244,7 +244,7 @@ class VisibilityBasedPreconditioner : public LinearOperator { // Temporary vector used by RightMultiply. cholmod_dense* tmp_rhs_; - DISALLOW_COPY_AND_ASSIGN(VisibilityBasedPreconditioner); + CERES_DISALLOW_COPY_AND_ASSIGN(VisibilityBasedPreconditioner); }; #else // SuiteSparse // If SuiteSparse is not compiled in, the preconditioner is not @@ -261,7 +261,7 @@ class VisibilityBasedPreconditioner : public LinearOperator { 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) { + bool Update(const BlockSparseMatrixBase& A, const double* D) { return false; } }; diff --git a/extern/libmv/third_party/ceres/patches/collections_port.h.mingw.patch b/extern/libmv/third_party/ceres/patches/collections_port.h.mingw.patch index bbb366e22bc..c01a17c7992 100644 --- a/extern/libmv/third_party/ceres/patches/collections_port.h.mingw.patch +++ b/extern/libmv/third_party/ceres/patches/collections_port.h.mingw.patch @@ -1,10 +1,10 @@ -Index: internal/ceres/collections_port.h -=================================================================== ---- internal/ceres/collections_port.h (revision 47730) -+++ internal/ceres/collections_port.h (working copy) -@@ -53,7 +53,7 @@ +diff --git a/internal/ceres/collections_port.h b/internal/ceres/collections_port.h +index a356cc0..c2fce90 100644 +--- a/internal/ceres/collections_port.h ++++ b/internal/ceres/collections_port.h +@@ -77,7 +77,7 @@ struct HashMap : std::tr1::unordered_map<K, V> {}; template<typename K> - struct HashSet : tr1::unordered_set<K> {}; + struct HashSet : std::tr1::unordered_set<K> {}; -#ifdef _WIN32 +#if defined(_WIN32) && !defined(__MINGW64__) && !defined(__MINGW32__) diff --git a/extern/libmv/third_party/ceres/patches/msvc_isfinite.patch b/extern/libmv/third_party/ceres/patches/msvc_isfinite.patch deleted file mode 100644 index c3129d8e02b..00000000000 --- a/extern/libmv/third_party/ceres/patches/msvc_isfinite.patch +++ /dev/null @@ -1,15 +0,0 @@ -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 index dbe955ae61e..3e65145696d 100644 --- a/extern/libmv/third_party/ceres/patches/series +++ b/extern/libmv/third_party/ceres/patches/series @@ -1 +1 @@ -msvc_isfinite.patch +collections_port.h.mingw.patch |