diff options
Diffstat (limited to 'extern/libmv/third_party/ceres')
115 files changed, 7254 insertions, 2422 deletions
diff --git a/extern/libmv/third_party/ceres/CMakeLists.txt b/extern/libmv/third_party/ceres/CMakeLists.txt index 06458834af3..56fd4c25473 100644 --- a/extern/libmv/third_party/ceres/CMakeLists.txt +++ b/extern/libmv/third_party/ceres/CMakeLists.txt @@ -40,22 +40,28 @@ set(INC_SYS set(SRC internal/ceres/array_utils.cc + internal/ceres/blas.cc internal/ceres/block_evaluate_preparer.cc internal/ceres/block_jacobian_writer.cc internal/ceres/block_jacobi_preconditioner.cc + internal/ceres/block_random_access_crs_matrix.cc internal/ceres/block_random_access_dense_matrix.cc internal/ceres/block_random_access_matrix.cc internal/ceres/block_random_access_sparse_matrix.cc internal/ceres/block_sparse_matrix.cc internal/ceres/block_structure.cc internal/ceres/canonical_views_clustering.cc + internal/ceres/c_api.cc internal/ceres/cgnr_solver.cc + internal/ceres/compressed_col_sparse_matrix_utils.cc internal/ceres/compressed_row_jacobian_writer.cc internal/ceres/compressed_row_sparse_matrix.cc internal/ceres/conditioned_cost_function.cc internal/ceres/conjugate_gradients_solver.cc internal/ceres/coordinate_descent_minimizer.cc internal/ceres/corrector.cc + internal/ceres/covariance.cc + internal/ceres/covariance_impl.cc internal/ceres/cxsparse.cc internal/ceres/dense_normal_cholesky_solver.cc internal/ceres/dense_qr_solver.cc @@ -67,7 +73,9 @@ set(SRC internal/ceres/generated/schur_eliminator_d_d_d.cc internal/ceres/gradient_checking_cost_function.cc internal/ceres/implicit_schur_complement.cc + internal/ceres/incomplete_lq_factorization.cc internal/ceres/iterative_schur_complement_solver.cc + internal/ceres/lapack.cc internal/ceres/levenberg_marquardt_strategy.cc internal/ceres/linear_least_squares_problems.cc internal/ceres/linear_operator.cc @@ -111,10 +119,12 @@ set(SRC include/ceres/autodiff_cost_function.h include/ceres/autodiff_local_parameterization.h + include/ceres/c_api.h include/ceres/ceres.h include/ceres/conditioned_cost_function.h include/ceres/cost_function.h include/ceres/cost_function_to_functor.h + include/ceres/covariance.h include/ceres/crs_matrix.h include/ceres/dynamic_autodiff_cost_function.h include/ceres/fpclassify.h @@ -146,6 +156,7 @@ set(SRC internal/ceres/block_evaluate_preparer.h internal/ceres/block_jacobian_writer.h internal/ceres/block_jacobi_preconditioner.h + internal/ceres/block_random_access_crs_matrix.h internal/ceres/block_random_access_dense_matrix.h internal/ceres/block_random_access_matrix.h internal/ceres/block_random_access_sparse_matrix.h @@ -156,11 +167,13 @@ set(SRC internal/ceres/cgnr_linear_operator.h internal/ceres/cgnr_solver.h internal/ceres/collections_port.h + internal/ceres/compressed_col_sparse_matrix_utils.h internal/ceres/compressed_row_jacobian_writer.h internal/ceres/compressed_row_sparse_matrix.h internal/ceres/conjugate_gradients_solver.h internal/ceres/coordinate_descent_minimizer.h internal/ceres/corrector.h + internal/ceres/covariance_impl.h internal/ceres/cxsparse.h internal/ceres/dense_jacobian_writer.h internal/ceres/dense_normal_cholesky_solver.h @@ -175,8 +188,10 @@ set(SRC internal/ceres/graph_algorithms.h internal/ceres/graph.h internal/ceres/implicit_schur_complement.h + internal/ceres/incomplete_lq_factorization.h internal/ceres/integral_types.h internal/ceres/iterative_schur_complement_solver.h + internal/ceres/lapack.h internal/ceres/levenberg_marquardt_strategy.h internal/ceres/linear_least_squares_problems.h internal/ceres/linear_operator.h @@ -186,7 +201,6 @@ set(SRC internal/ceres/line_search_minimizer.h internal/ceres/low_rank_inverse_hessian.h internal/ceres/map_util.h - internal/ceres/matrix_proto.h internal/ceres/minimizer.h internal/ceres/mutex.h internal/ceres/parameter_block.h @@ -206,6 +220,7 @@ set(SRC internal/ceres/schur_eliminator_impl.h internal/ceres/schur_jacobi_preconditioner.h internal/ceres/scratch_evaluate_preparer.h + internal/ceres/small_blas.h internal/ceres/solver_impl.h internal/ceres/sparse_matrix.h internal/ceres/sparse_normal_cholesky_solver.h @@ -261,7 +276,7 @@ add_definitions( -DCERES_HAVE_PTHREAD -DCERES_NO_SUITESPARSE -DCERES_NO_CXSPARSE - -DCERES_NO_PROTOCOL_BUFFERS + -DCERES_NO_LAPACK -DCERES_RESTRICT_SCHUR_SPECIALIZATION -DCERES_HAVE_RWLOCK ) diff --git a/extern/libmv/third_party/ceres/ChangeLog b/extern/libmv/third_party/ceres/ChangeLog index 1a945b01622..6bb33068b2a 100644 --- a/extern/libmv/third_party/ceres/ChangeLog +++ b/extern/libmv/third_party/ceres/ChangeLog @@ -1,717 +1,638 @@ -commit 36f4cd23b24391106e9f3c15b0f9bbcaafc47b20 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun Apr 21 09:42:26 2013 -0700 +commit 682cd3c27864ba6d67ca81890760a5f697f21d63 +Author: Keir Mierle <mierle@gmail.com> +Date: Tue Sep 3 14:28:32 2013 -0700 - Disable threads completely if OpenMP is not present. - - This reduces the penalty paid by Mutex lock and unlock operations - in single threaded mode. + Update version history with shared libs changes - Change-Id: I185380bde73fe87e901fc434d152d6c366ff1d5d + Change-Id: Iafd55087bc5eef4c15c3b544222147aa99df7690 -commit 24fb32b42683cf711a6683e3cff3540b16bb5019 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sat Apr 20 09:02:06 2013 -0700 +commit 340d7c1415f144ca335ec1e87832c3f41d5d515b +Author: Keir Mierle <mierle@gmail.com> +Date: Tue Sep 3 13:50:03 2013 -0700 - Add whole program optimization for Clang. + Update version history with miniglog fix - Also reorder the way CERES_CXX_FLAGS is being used for clarity. - - Change-Id: I2bbb90e770d30dd18ecae72939ea03b7fa11e6ae + Change-Id: Ic69f4994259e05fa88548b957146a1aac73b7af7 -commit 2b7497025096a681d7f0351081f83293398d62ef -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Fri Apr 19 19:52:58 2013 -0700 +commit ac061c0f2334868e671f26d24e34a14c77fac716 +Author: Keir Mierle <mierle@gmail.com> +Date: Tue Sep 3 13:03:28 2013 -0700 - Fix a bounds error in the pre-ordering code. + Cleanups in logging.h + + Thanks to Scott Ettinger for the patch this is based off of, + which restores the NDK build. - Change-Id: I33c968bb075b60ad50374593302e08f42aeacf25 + Change-Id: I8036dc1388438a4940e6f4ae297162902afd8d3a -commit 9189f4ea4bb2d71ea7f6b9d9bd3290415aee323d +commit 0338f9a8e69582a550ef6d128e447779536d623c Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Fri Apr 19 17:09:49 2013 -0700 +Date: Mon Sep 2 22:28:40 2013 -0700 - Enable pre-ordering for SPARSE_NORMAL_CHOLESKY. - - Sparse Cholesky factorization algorithms use a fill-reducing - ordering to permute the columns of the Jacobian matrix. There - are two ways of doing this. - - 1. Compute the Jacobian matrix in some order and then have the - factorization algorithm permute the columns of the Jacobian. - - 2. Compute the Jacobian with its columns already permuted. - - The first option incurs a significant memory penalty. The - factorization algorithm has to make a copy of the permuted - Jacobian matrix. - - Starting with this change Ceres pre-permutes the columns of the - Jacobian matrix and generally speaking, there is no performance - penalty for doing so. - - In some rare cases, it is worth using a more complicated - reordering algorithm which has slightly better runtime - performance at the expense of an extra copy of the Jacobian - matrix. Setting Solver::Options::use_postordering to true - enables this tradeoff. + ITERATIVE_SCHUR works with no f-blocks. - This change also removes Solver::Options::use_block_amd - as an option. All matrices are ordered using their block - structure. The ability to order them by their scalar - sparsity structure has been removed. + When the Schur complement is of size zero, + i.e. none of the parameter blocks interact + with each other, the ITERATIVE_SCHUR linear + solver crashes due to some checks that are + triggered in the SCHUR_JACOBI preconditioner. - Here is what performance on looks like on some BAL problems. + This patch adds logic to detect this condition + and to deal with it and adds tests that verify + the fix. - Memory - ====== - HEAD pre-ordering - 16-22106 137957376.0 113516544.0 - 49-7776 56688640.0 46628864.0 - 245-198739 1718005760.0 1383550976.0 - 257-65132 387715072.0 319512576.0 - 356-226730 2014826496.0 1626087424.0 - 744-543562 4903358464.0 3957878784.0 - 1024-110968 968626176.0 822071296.0 + Thanks to Soohyun Bae for reporting this bug. - Time - ==== - HEAD pre-ordering - 16-22106 3.8 3.7 - 49-7776 1.9 1.8 - 245-198739 82.6 81.9 - 257-65132 14.0 13.4 - 356-226730 98.8 95.8 - 744-543562 325.2 301.6 - 1024-110968 42.1 37.1 - - Change-Id: I6b2e25f3fed7310f88905386a7898ac94d37467e + Change-Id: If29ddf32463cbb1960414fff0e29bbf0d2ee7989 -commit f7ed22efc3afee36aae71a4f7949b3d327b87f11 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Fri Apr 19 14:24:48 2013 -0700 +commit 263de47419167786c9ab6d93fa2f3e32e8e75fe1 +Author: Taylor Braun-Jones <taylor@braun-jones.org> +Date: Thu Aug 29 10:33:29 2013 -0400 - Add the ability to order the Program using AMD. + Incorporate RHEL build fixes from Brian Pitts - This will allow CHOLMOD to compute the sparse - Cholesky factorization of J'J without making - a permuted copy of it. + CMake build fixed so that versioned shared libraries are installed + (along with .so symlinks) - Change-Id: I25d0e18f5957ab7fdce15c543234bb2f09db482e + Change-Id: Ibbaea9d37d17754cb8c3cd36fc17d015ca7d2a57 -commit c8f07905d76d9ac6fb8d7b9b02e180aa2fa0ab32 +commit 6b4131993ec0db6c850bb2ae07ba8793dbab3e39 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Fri Apr 19 08:01:04 2013 -0700 +Date: Mon Aug 26 00:02:50 2013 -0700 - Refactor SolverImpl::CreateReducedProgram. + Update spec file - Break up CreateReducedProgram into smaller functions in - preparation for more sophisticated ordering strategies. - - Change-Id: Ic3897522574fde770646d747fe383f5dbd7a6619 + Change-Id: Id6426d7cad41cde2cbab411964ac013d724a066c -commit 2560b17b7cdda1de28c18049c95e6c3188dbde93 +commit c24a4ec6fb6202d1f6a576f211b99fbe9c9906ef Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Fri Apr 19 08:19:11 2013 -0700 +Date: Fri Aug 23 06:49:22 2013 -0700 - SuiteSparse cleanup. - - 1. CreateSparseMatrixTransposeView now returns a struct instead - of a pointer. + Cmake refactoring - 2. Add AnalyzeCholeskyWithNaturalOrdering. + 1. Use CMake FindLAPACK and FindBLAS Modules. + 2. Remove SEARCH_HEADERS and SEARCH_LIBS and replace them with + CMAKE variables. This leads to simplification of the FIND_LIBRARY + and FIND_PATH calls. + 3. Make miniglog a fallback when glog is not present and the + user indicates MINIGLOG=OFF. + 4. Add time.h to miniglog. + 5. Remove shared library building. - Change-Id: If27a5502949c3994edd95be0d25ec7a0d1fa1ae1 + Change-Id: I8a97156d3d7cf645fbbfe8e571761bc16c89f43f -commit 7823cf23c765450b79f11ac31fc8a16f875c0d84 +commit 48e9cd31db0bf7223beb83cdc90e3cd2b5aad054 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu Apr 18 16:13:56 2013 -0700 +Date: Wed Aug 21 10:55:16 2013 -0700 - Fix a typo in problem.h - - Thanks as usual to William Rucklidge. + Add a test name - Change-Id: If6e8628841ee7fa8978ec56918a80d60b4ff660e + Change-Id: I06dfc9cad2c54ef6078342766577eab92645283f -commit 3d9546963d7c8c5f5dfb12a2df745f4996fd2ec5 +commit 126dfbe27df9c5b9f41cf7cc92b75c1219518283 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu Apr 18 14:54:55 2013 -0700 +Date: Tue Aug 20 22:34:34 2013 -0700 - Add the ability to query the Problem about parameter blocks. + Fix how Ceres calls CAMD. - Change-Id: Ieda1aefa28e7a1d18fe6c8d1665882e4d9c274f2 - -commit 69ebad42ebfc212339a22c6f06a12ec5a3368098 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Apr 17 15:38:00 2013 -0700 - - Change Minimizer::Options::min_trust_region_radius to double. + CAMD requires that the id of the largest numbered elimination + group be less than the number of columns in the matrix. - This was accidentally an int, which was setting the minimum - trust region radius to zero and effectively disabling a convergence - test based on it. + This patch ensures that this is the case. Without this, + in certain cases its possible for CAMD to silently fail + while doing out of bounds access and then causing Ceres to fail. - (Thanks to Sergey Sharybin for providing a reproduction for this) + Also add some logging about the problem size before and after + the reduced program has been created. - Change-Id: Id0b9e246bcfee074954a5dc6a3a2342adab56c16 + Change-Id: I0ea3c6572a7c29cbbf09afec9ba5b4f4d4b21a9b -commit e6707b2411b9a823b6c748f9f9d0b22225d767bb +commit 69af5d8b4d7c48b2efa3c61e51c86cfa1b380b8a Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Apr 16 15:44:23 2013 -0700 +Date: Tue Aug 20 13:58:59 2013 -0700 - Lint fixes from William Rucklidge. + Add comments to trust_region_minimizer.cc. - Change-Id: I57a6383bb875b24083cd9b7049333292d26f718c - -commit c7e69beb52c2c47182eaf8295025a668d0eefd80 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Apr 16 09:39:16 2013 -0700 - - Add a missing mutex lock in the SchurEliminator. This - was lost somewhere along in the BLAS based refactoring. + trust_region_minimizer.cc now contains a comment that explains + the reasoning behind he inner iteration step acceptance change. - Change-Id: I90b94fa9c3a8ea1b900a18f76ef6a7d0dbf24318 + Change-Id: I4eaa69d6bab92c543bba3f119c09f44625d393bd -commit faa72ace9abea24877173158bfec451d5b46597e -Author: Joydeep Biswas <joydeep.biswas@gmail.com> -Date: Mon Apr 15 17:34:43 2013 -0400 - - Update to compile with stricter gcc checks. - - Change-Id: Iecb37cbe7201a4d4f42b21b427fa1d35d0183b1b - -commit 487250eb27256a41d38c5037bdac9a09a3160edb +commit e45db9d05aaa26b1ddffa44c9190a1018aa2655f Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Fri Apr 5 14:20:37 2013 -0700 +Date: Mon Aug 19 23:13:29 2013 -0700 - Minor cleanups. - - 1. Further BLAS and heap allocation cleanups in schur_eliminator_impl.h - 2. Modularize blas.h using macros. - 3. Lint cleanups from William Rucklidge. - 4. Small changes to jet.h - 5. ResidualBlock now uses blas.h - - Performance improvements: - - For static and dynamic sized blocks, the peformance is not changed much. + Improve inner iteration step acceptance. - -use_quaternions -ordering user -linear_solver sparse_schur + Normally, in a trust region algorithm the quality of a trust region step + is measured by the ratio - master change - problem: 16-22106 - gcc 3.4 3.3 - clang 2.8 2.7 + nonlinear_cost_change + r = --------------------- + model_cost_change - problem: 49-7776 - gcc 1.7 1.7 - clang 1.4 1.4 + All the change in the nonlinear objective is due to the trust region step + so this ratio is a good measure of the quality of the trust region radius. - problem: 245-198739 - gcc 80.1 79.6 - clang 80.6 76.2 + However, when inner iterations are being used, nonlinear_cost_change + includes the contribution of the inner iterations and its not fair to + credit it all to the trust region algorithm. So we change the ratio to be - problem: 257-65132 - gcc 12.2 12.0 - clang 10.4 10.2 + nonlinear_cost_change + r = ------------------------------------------------ + (model_cost_change + inner_iteration_cost_change) - problem: 356-226730 - gcc 99.0 96.8 - clang 88.9 88.3 + In most cases this is fine, but it can be the case that the + change in solution quality due to inner iterations is so large + and the trust region step is so bad, that this ratio can become + quite small. - problem: 744-543562 - gcc 361.5 356.2 - clang 352.7 343.5 + This can cause the trust region loop to reject this step. - problem: 1024-110968 - gcc 45.9 45.6 - clang 42.6 42.1 + This change, fixes this problem by looking at the inner_iteration_cost_change + explicitly and accepting a step if the inner iterations led to a net + decrease in the objective function value. - However, performance when using local parameterizations is - significantly improved due to residual_block.cc using blas.h + Along the way it also fixes the way model_cost_change is computed. + Changing to a more numerically robust way of computing it. - -use_quaternions -use_local_parameterization -ordering user -linear_solver sparse_schur + The last and final change is to ensure that inner iterations and the + non-monotonic version of the trust region algorithm interact correctly. - master change - problem: 16-22106 - gcc 3.6 3.3 - clang 3.5 2.8 + This addresses part 2 of - problem: 49-7776 - gcc 1.8 1.6 - clang 1.7 1.4 + https://code.google.com/p/ceres-solver/issues/detail?id=115 - problem: 245-198739 - gcc 79.7 76.1 - clang 79.7 73.0 + As an illustration of the change. - problem: 257-65132 - gcc 12.8 11.9 - clang 12.3 9.8 + Before this change - problem: 356-226730 - gcc 101.9 93.5 - clang 105.0 86.8 + [master] build: ./bin/bundle_adjuster --input ~/Downloads/problem-245-198739-pre.txt -num_iterations 10 -translation_sigma 0.01 -rotation_sigma 0.001 -point_sigma 0.1 -inner_iterations -num_threads 4 + 0: f: 7.731660e+15 d: 0.00e+00 g: 3.51e+12 h: 0.00e+00 rho: 0.00e+00 mu: 1.00e+04 li: 0 it: 5.87e-01 tt: 9.37e+00 + 1: f: 7.731660e+15 d: 7.73e+15 g: 0.00e+00 h: 1.20e+10 rho: 2.43e-11 mu: 5.00e+03 li: 1 it: 1.41e+01 tt: 2.35e+01 + 2: f: 7.731660e+15 d: 7.73e+15 g: 0.00e+00 h: 1.25e+10 rho: 1.70e-07 mu: 1.25e+03 li: 1 it: 1.86e+01 tt: 4.22e+01 + 3: f: 7.731660e+15 d:-2.39e+40 g: 0.00e+00 h: 3.53e+10 rho:-2.63e-13 mu: 1.56e+02 li: 1 it: 3.35e+01 tt: 7.57e+01 + 4: f: 7.731660e+15 d:-1.66e+39 g: 0.00e+00 h: 1.21e+11 rho:-6.58e-15 mu: 9.77e+00 li: 1 it: 3.86e+01 tt: 1.14e+02 + 5: f: 7.731660e+15 d:-3.57e+55 g: 0.00e+00 h: 5.00e+12 rho:-1.89e-14 mu: 3.05e-01 li: 1 it: 3.84e+01 tt: 1.53e+02 + 6: f: 7.731660e+15 d:-2.26e+35 g: 0.00e+00 h: 3.82e+12 rho:-1.77e-20 mu: 4.77e-03 li: 1 it: 3.45e+01 tt: 1.87e+02 + 7: f: 7.731660e+15 d:-5.31e+19 g: 0.00e+00 h: 1.22e+11 rho:-9.96e-21 mu: 3.73e-05 li: 1 it: 2.77e+01 tt: 2.15e+02 + 8: f: 1.784990e+08 d: 7.73e+15 g: 4.13e+07 h: 1.20e+10 rho: 1.00e+00 mu: 1.12e-04 li: 1 it: 1.13e+01 tt: 2.26e+02 + 9: f: 1.524025e+08 d: 2.61e+07 g: 5.81e+10 h: 2.41e+08 rho: 1.00e+00 mu: 3.35e-04 li: 1 it: 1.13e+01 tt: 2.37e+02 + 10: f: 1.488524e+08 d: 3.55e+06 g: 2.79e+09 h: 5.01e+08 rho: 1.00e+00 mu: 1.01e-03 li: 1 it: 1.09e+01 tt: 2.48e+02 - problem: 744-543562 - gcc 367.9 350.5 - clang 355.3 323.1 + After this change - problem: 1024-110968 - gcc 43.0 40.3 - clang 41.0 37.5 + [inner] build: ./bin/bundle_adjuster --input ~/Downloads/problem-245-198739-pre.txt -num_iterations 10 -translation_sigma 0.01 -rotation_sigma 0.001 -point_sigma 0.1 -inner_iterations -num_threads 4 + 0: f: 7.731660e+15 d: 0.00e+00 g: 3.51e+12 h: 0.00e+00 rho: 0.00e+00 mu: 1.00e+04 li: 0 it: 5.66e-01 tt: 9.31e+00 + 1: f: 5.941477e+09 d: 7.73e+15 g: 1.20e+18 h: 1.20e+10 rho: 2.43e-11 mu: 5.00e+03 li: 1 it: 1.38e+01 tt: 2.32e+01 + 2: f: 3.341986e+08 d: 5.61e+09 g: 1.42e+14 h: 1.37e+09 rho: 9.38e-08 mu: 2.50e+03 li: 1 it: 1.30e+01 tt: 3.61e+01 + 3: f: 3.241492e+08 d: 1.00e+07 g: 3.64e+13 h: 8.26e+08 rho: 6.12e-08 mu: 1.25e+03 li: 1 it: 1.15e+01 tt: 4.77e+01 + 4: f: 3.152280e+08 d: 8.92e+06 g: 2.02e+13 h: 2.95e+08 rho: 1.56e-05 mu: 6.25e+02 li: 1 it: 1.11e+01 tt: 5.88e+01 + 5: f: 3.078535e+08 d: 7.37e+06 g: 9.72e+12 h: 4.57e+08 rho: 6.55e-09 mu: 3.13e+02 li: 1 it: 1.16e+01 tt: 7.04e+01 + 6: f: 3.025353e+08 d: 5.32e+06 g: 1.33e+13 h: 2.14e+08 rho: 7.21e-01 mu: 3.42e+02 li: 1 it: 1.14e+01 tt: 8.18e+01 + 7: f: 2.908298e+08 d: 1.17e+07 g: 5.97e+12 h: 7.25e+08 rho: 5.73e-01 mu: 3.43e+02 li: 1 it: 1.08e+01 tt: 9.26e+01 + 8: f: 2.803927e+08 d: 1.04e+07 g: 1.07e+12 h: 9.72e+07 rho: 5.27e-01 mu: 3.43e+02 li: 1 it: 1.03e+01 tt: 1.03e+02 + 9: f: 2.767074e+08 d: 3.69e+06 g: 2.10e+11 h: 7.35e+07 rho: 7.37e-01 mu: 3.84e+02 li: 1 it: 1.03e+01 tt: 1.13e+02 + 10: f: 2.744282e+08 d: 2.28e+06 g: 2.17e+11 h: 1.23e+08 rho: 3.11e-01 mu: 3.64e+02 li: 1 it: 9.61e+00 tt: 1.23e+02 - Change-Id: I6dcf7476ddaa77cb116558d112a9cf1e832f5fc9 + Change-Id: I7c3b132f7ce62719795bfa489ec2276d0455cc97 -commit eeedd3a59281eb27025d7f9aa944d9aff0666590 -Author: Sergey Sharybin <sergey.vfx@gmail.com> -Date: Wed Apr 10 23:58:32 2013 +0600 +commit 3e6ef29be6f3cd672a73cefb52838832a49e5427 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue Aug 20 09:53:54 2013 -0700 - Autodiff local parameterization class - - This class is used to create local parameterization - with Jacobians computed via automatic differentiation. - - To get an auto differentiated local parameterization, - class with a templated operator() (a functor) that - computes - - plus_delta = Plus(x, delta); - - shall be defined. + Update version history to reflect API changes - Then given such functor, the auto differentiated local - parameterization can be constructed as - - LocalParameterization* local_parameterization = - new AutoDiffLocalParameterization<PlusFunctor, 4, 3>; - | | - Global Size ---------------+ | - Local Size -------------------+ - - See autodiff_local_parameterization.h for more information - and usage example. - - Initial implementation by Keir Mierle, finished by self - and integrated into Ceres and covered with unit tests - by Sameer Agarwal. - - Change-Id: I1b3e48ae89f81e0cf1f51416c5696e18223f4b21 + Change-Id: I5ce744d72b991abba17b5cf9c6a1e1f158693151 -commit d8d541674da5f3ba7a15c4003fa18577479f8f8c -Author: Sergey Sharybin <sergey.vfx@gmail.com> -Date: Wed Apr 10 11:13:27 2013 +0600 +commit 1918453aeeae629be1f02eb333e91c4f728ace12 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Mon Aug 19 14:15:48 2013 -0700 - Do not modify cached CMAKE_CXX_FLAGS_RELEASE + Fix build breakage on old SuiteSparse. - Adding compiler's flags and force updating cached value - of release C++ flags lead to appending special compiler - flags on every edit of any CMakeList.txt. + Errant semi colon is to blame. - For compile result this is harmless, but was annoying - slight modification of CMakeList.txt triggered full - project rebuild. + Thanks to Timothy Langlois for reporting this. - Now modified C++ flags are used for the whole subtree - starting from the project root, but this doesn't lead - to flags modified in cache. - - Change-Id: Ieb32bd7f96d5a09632f0b2b5325f6567be8cb5a8 + Change-Id: I57bb1cd69d78ab1897ead3627539a0da11b97455 -commit c290df85a40a8dd117b5eccc515bf22b0d9b1945 +commit 8f33332c598d8209df73eb1c729e0abe2c890468 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun Apr 7 09:17:47 2013 -0700 +Date: Sun Aug 18 23:25:00 2013 -0700 - Typo fix. - - (Thanks to Pieree Moulon for reporting this) + Documentation update for 1.7.0rc2 - Change-Id: I536724ab4b7e9c97768d5197aa86b41f37a04d38 + Change-Id: I6b0c19bed57b51a0f6591c60a4ae0d849c62451b -commit dc3a27fa60ba7c6b152660afd5abe1c8b608dec3 +commit ad2819a1afa94990022999a96eb158add68419e0 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sat Apr 6 19:32:47 2013 -0700 +Date: Sat Aug 17 23:44:09 2013 -0700 - Fix MatrixVectorMultiply and incorrect DCHECKS. + Fix breakage on old versions of SuiteSparse. - (Thanks to Serget Sharybin for reporting this) + Thanks to Fisher Yu for reporting this. - Change-Id: I6bbc41667308fc2932871cf25ad07b431f70801f + Change-Id: Iefa89816cbb60e3512338a7c2a65655c017877ac -commit 585607171f20d591033dfea43b6dd22fea755a6c +commit 880cba0939b2caa2641a5752373ffd47b64edd0f Author: Petter Strandmark <petter.strandmark@gmail.com> -Date: Sun Apr 7 01:24:13 2013 +0200 +Date: Fri Aug 16 20:05:30 2013 +0200 - <iterator> needed for back_insert_iterator + Fix warning C4373 in Visual Studio - Adding this header was required to make Ceres compile with VS2010. + The warning occurs because an overridden function added a const + to one argument. - Change-Id: I000c860da4fd385d625e70695564225bdfd433c7 + Change-Id: Idd24f7c6ab60064747104bfc75ae9bf112f61b3e -commit 520d35ef22dbcb05e344451c03ae64304e524a06 +commit d61b68aaac3fa51b8fca8b1a268e83b0d5da01ea Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu Apr 4 08:16:02 2013 -0700 +Date: Fri Aug 16 17:02:56 2013 -0700 - Further BLAS improvements. - - 1. Switch to Eigen's implementation when all dimensions are fixed. - 2. Use lazyProduct for eigen matrix-vector product. This brings - eigen's performance on iterative_schur closer to what it used - to be before the last commit. There is however still an - improvement to be had by using the naive implementation when - the matrix and vector have dynamic dimensions. - - BENCHMARK - HEAD CHANGE - - problem-16-22106-pre.txt - gcc-eigen sparse_schur 0.859 gcc-eigen sparse_schur 0.853 - clang-eigen sparse_schur 0.848 clang-eigen sparse_schur 0.850 - gcc-blas sparse_schur 0.956 gcc-blas sparse_schur 0.865 - clang-blas sparse_schur 0.954 clang-blas sparse_schur 0.858 - gcc-eigen iterative_schur 4.656 gcc-eigen iterative_schur 3.271 - clang-eigen iterative_schur 4.664 clang-eigen iterative_schur 3.307 - gcc-blas iterative_schur 2.598 gcc-blas iterative_schur 2.620 - clang-blas iterative_schur 2.554 clang-blas iterative_schur 2.567 + Lint cleanups from William Rucklidge - problem-49-7776-pre.txt - gcc-eigen sparse_schur 0.477 gcc-eigen sparse_schur 0.472 - clang-eigen sparse_schur 0.475 clang-eigen sparse_schur 0.479 - gcc-blas sparse_schur 0.521 gcc-blas sparse_schur 0.469 - clang-blas sparse_schur 0.508 clang-blas sparse_schur 0.471 - gcc-eigen iterative_schur 3.172 gcc-eigen iterative_schur 2.088 - clang-eigen iterative_schur 3.161 clang-eigen iterative_schur 2.079 - gcc-blas iterative_schur 1.701 gcc-blas iterative_schur 1.720 - clang-blas iterative_schur 1.708 clang-blas iterative_schur 1.694 - - problem-245-198739-pre.txt - gcc-eigen sparse_schur 28.092 gcc-eigen sparse_schur 28.233 - clang-eigen sparse_schur 28.148 clang-eigen sparse_schur 28.400 - gcc-blas sparse_schur 30.919 gcc-blas sparse_schur 28.110 - clang-blas sparse_schur 31.001 clang-blas sparse_schur 28.407 - gcc-eigen iterative_schur 63.095 gcc-eigen iterative_schur 43.694 - clang-eigen iterative_schur 63.412 clang-eigen iterative_schur 43.473 - gcc-blas iterative_schur 33.353 gcc-blas iterative_schur 33.321 - clang-blas iterative_schur 33.276 clang-blas iterative_schur 33.278 + Change-Id: Ia4756ef97e65837d55838ee0b30806a234565bfd + +commit b22d063075ec545a59a25abd5d83e4642dc329c2 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu Aug 15 22:55:23 2013 -0700 + + Reduce memory usage in covariance estimation. - problem-257-65132-pre.txt - gcc-eigen sparse_schur 3.687 gcc-eigen sparse_schur 3.629 - clang-eigen sparse_schur 3.669 clang-eigen sparse_schur 3.652 - gcc-blas sparse_schur 3.947 gcc-blas sparse_schur 3.673 - clang-blas sparse_schur 3.952 clang-blas sparse_schur 3.678 - gcc-eigen iterative_schur 121.512 gcc-eigen iterative_schur 76.833 - clang-eigen iterative_schur 123.547 clang-eigen iterative_schur 78.763 - gcc-blas iterative_schur 68.334 gcc-blas iterative_schur 68.612 - clang-blas iterative_schur 67.793 clang-blas iterative_schur 68.266 + When using the SPARSE_QR algorithm, now a Q-less + factorization is used. This results in significantly + less memory usage. - Notes: + The inversion of the semi-normal equations is now + threaded using openmp. Indeed if one has SuiteSparse + compiled with TBB, then both the factorization + and the inversion are completely threaded. - 1. Naive BLAS was a bit worse than eigen on fixed sized matrices. We did not see this - before because of the different inlining thresholds. Fixing this boosted eigen's - performance. Also the disparity between gcc and clang has gone away. + Change-Id: Ia07591e48e7958d427ef91ff9e67662f6e982c21 + +commit f258e4624f5bd86105ea28b9b92dd70a3f4a3a44 +Author: Sergey Sharybin <sergey.vfx@gmail.com> +Date: Thu Aug 15 14:50:08 2013 +0600 + + Move most of suitesparse/cxsparse ifdef code to their headers - 2. SPARSE_SCHUR performance remains the same, since it is only testing static sized - matrices. + Main purpose of this is to make implementation files free from + endless ifdef blocks every time this libraries are needed to be + included. This would hopefully prevent compilation errors in + the future caused by missing ifdef around header include. - 3. ITERATIVE_SCHUR performance goes up substantially due to the lazyProduct change, - but even there, since most of the products are dynamic sized, the naive implementation - wins handily. + This also includes some stubs added to suitesparse/cxsparse + headers to make code even more free from ifdefs. - Change-Id: Idc17f35b9c68aaebb1b2e131adf3af8374a85a4c + Change-Id: Ic8554e7df31d8c4751583fe004b99e71b3c9087b -commit 25ac54807eedf16fd6c34efc390901ee549a7d14 +commit dc60d9c4519b5eb5e2cff8741680fecf4d6eb2c5 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Apr 3 18:51:27 2013 -0700 +Date: Thu Aug 15 10:13:45 2013 -0700 - Speed up Jets. + Fix broken android build. - Change-Id: I101bac1b1a1cf72ca49ffcf843b73c0ef5a6dfcb + Change-Id: I6f27e3ef9bd678f7393c9f573491064978e9c368 -commit 3d6eceb45cf27024865908f0c10a5c2b0f8719cf +commit 367b65e17a541a9f29b9ea63682fe6f6b5b54074 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Apr 2 21:45:48 2013 -0700 +Date: Fri Aug 9 10:35:37 2013 -0700 - Replace more instances of Eigen GEMV with Ceres BLAS. + Multiple dense linear algebra backends. + + 1. When a LAPACK implementation is present, then + DENSE_QR, DENSE_NORMAL_CHOLESKY and DENSE_SCHUR + can use it for doing dense linear algebra operations. + + 2. The user can switch dense linear algebra libraries + by setting Solver::Options::dense_linear_algebra_library_type. - With this ITERATIVE_SCHUR with JACOBI preconditioner went down from - 280 seconds to 150 seconds on problem-744-543562-pre.txt. + 3. Solver::Options::sparse_linear_algebra_library is now + Solver::Options::sparse_linear_algebra_library_type to be consistent + with all the other enums in Solver::Options. - Change-Id: I4f319c1108421e8d59f58654a4c0576ad65df609 + 4. Updated documentation as well as Solver::Summary::FullReport + to reflect these changes. + + Change-Id: I5ab930bc15e90906b648bc399b551e6bd5d6498f -commit 296fa9b1279ee1900c8ae32d70e97cd10fc0b46b +commit 080d1d04bdf722c3f602833c4c07ac1c5d26fcc0 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Apr 2 09:44:15 2013 -0700 +Date: Mon Aug 12 16:28:37 2013 -0700 - Replace Eigen block operations with small GEMM and GEMV loops. + Use more performant, less conservative Eigen solvers. + + colPivHouseholderQR -> householderQR + ldlt -> llt. + + The resulting performance differences are significant enough + to justify switching. - 1. Add Matrix-Matrix and Matrix-Vector multiply functions. - 2. Replace Eigen usage in SchurEliminator with these custom - matrix operations. - 3. Save on some memory allocations in ChunkOuterProduct. - 4. Replace LDLT with LLT. + LAPACK's dgels routine used for solving linear least squares + problems does not use pivoting either. - As a result on problem-16-22106-pre.txt, the linear solver time - goes down from 1.2s to 0.64s. + Similarly, we are not actually using the fact that the matrix + being factorized can be indefinite when using LDLT factorization, so + its not clear that the performance hit is worth it. - Change-Id: I2daa667960e0a1e8834489965a30be31f37fd87f + These two changes result in Eigen being able to use blocking + algorithms, which for Cholesky factorization, brings the performance + closer to hardware optimized LAPACK. Similarly for dense QR + factorization, on intel there is a 2x speedup. + + Change-Id: I4459ee0fc8eb87d58e2b299dfaa9e656d539dc5e -commit 222ca20e8facf706582fe696b7f41247391eac12 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Apr 1 09:11:07 2013 -0700 +commit fb465a03b83fad2dceaea091ee3763c3dc6e83d2 +Author: Sergey Sharybin <sergey.vfx@gmail.com> +Date: Mon Aug 5 22:35:14 2013 -0700 - SuiteSparse cleanup. + Fix compilation error caused by missing suitesparse headers - 1. Silence CHOLMOD's indefiniteness warnings. - 2. Add a comment about how the error handling in suitesparse.cc - needs to be improved. - 3. Move the analysis logging into suitesparse.cc and out of the - three callsites. + Covariance implementation file used to unconditionally include + SuiteSparseQR.hpp which caused compilation error in cases you + don't have SuiteSuite installed to the system - Change-Id: Idd396b8ea4bf59fc1ffc7f9fcbbc7b38ed71643c + Moved the include to #ifdef block. + + Change-Id: I3a52c0f81711b2b70ae625fe80b758ecb0817cc6 -commit b7ba93459b7f584eedb1a9f42f3d06bccabd33dc -Author: Petter Strandmark <petter.strandmark@gmail.com> -Date: Tue Feb 19 12:52:58 2013 +0100 +commit 2460bf0733b4070e52d68a4a85046c1b20913e2c +Author: Steven Lovegrove <stevenlovegrove@gmail.com> +Date: Sun Jul 21 13:13:11 2013 -0400 - Enable larger tuple sizes for Visual Studio 2012. + Check GCC Version before adding -fast compiler option on OSX. - Visual Studio 2012 does not have variadic templates and implements - tuples differently. By default, only sizes up to 5 are supported, - which conflicts with Gtest. + -fast compiler option is only supported using Apple's GCC packaged with XCode. + Other GCC versions will fail when this flag is enabled. This commit checks the + GCC version on OSX and only enables this flag when < 4.3. Apple's GCC is + currently 4.2.1 and a user is unlikely to install a non-apple version this old + on OSX. - Change-Id: Ieb8d59e4329863cbfa2729d8a76db0714c08d259 + Change-Id: Ifca9149625c065cd16559d7e30c218a322cf79aa -commit 564a83fcc690ac8383bf52a782c45757ae7fa2ad +commit c5bcfc01af37b4f667be075c3c58dc024f3c7f06 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Mar 26 11:11:43 2013 -0700 +Date: Fri Jul 19 15:50:27 2013 -0700 - Lint cleanup from William Rucklidge. + Lint fixes from Jim Roseborough. - Change-Id: I8d4a0aa3e264775d20e99a6b5265f3023de92560 + Change-Id: If93e1972041b36410225a509e3c8c7c818f92124 -commit cbe64827becbbaab5b435a71bf00353b4ddd026b +commit 16924168ce0b3e29d9b1e16a08d2b3d2930e017a Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Mar 25 17:39:53 2013 -0700 +Date: Thu Jul 18 12:52:35 2013 -0700 - Update documentation + Update version from 1.6.0 -> 1.7.0rc1. - Change-Id: Iea3c4b5409e593b1fb070a491ba8a479db32ca58 + Change-Id: I420a8907142bffad0e3aa6c7196541ca2309c099 -commit 802639c89603c9541e624766349d1989a1f641c0 -Author: Pablo Speciale <pablo.speciale@gmail.com> -Date: Mon Mar 25 20:53:45 2013 -0700 +commit 588228bdadcc0a1ffc55442a0672998241e53e09 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu Jul 18 11:29:19 2013 -0700 - ceresproblem label was incorrect + Add the ability to turn shared library compilation on and off - Change-Id: I3e210375adba4fa50ff3c25398b20a65d241cb35 + Change-Id: Ib9eacfbc894bb2b66aafff3b930c63e2ad8a555e -commit 6bcb8d9c304a3b218f8788018dfdfe368bb7d60c -Author: Pablo Speciale <pablo.speciale@gmail.com> -Date: Mon Mar 25 16:40:26 2013 -0700 +commit 6d93450cb563dc992cbc29ca069c886bf24bb458 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu Jul 18 11:08:07 2013 -0700 - Compiling against static or shared library + Fix build breakage on old versions of SuiteSparse. + + SuiteSparse_long is only defined in recent versions of SuiteSparse + as the index variable type for large matrices. In older versions + UF_long was used. Ubuntu still ships with an older version of + SuiteSparse, so an ifdef is needed to fix the build. + + This patch has been tested on mac and on linux with older and + newer versions of SuiteSparse. - Change-Id: I3fb35e9a49f90b8894f59dde49c90a7c2dd74b0a + Change-Id: I4ada86d7973784a79bde4afec13ce3ca4e8dc225 -commit 619cfe692020c078275b68eac2167232fafdfffb +commit 42be9cafe6203745fb09d611773305433c117396 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Mar 25 14:03:41 2013 -0700 +Date: Thu Jul 18 08:02:08 2013 -0700 - Update version history + Update documentation for Covariance - Change-Id: I1d036efad1507efd19d8581f147b38170b1f0543 + Change-Id: Ia4a7347ef8267b7107698d85fcbfc986111958dc -commit 6ae34757850a5fa8213e0d1a540d9d75d6840a08 -Author: Pablo Speciale <pablo.speciale@gmail.com> -Date: Sun Mar 24 22:30:52 2013 -0700 +commit 5a974716e111e4aa87a4840902b957060bd644fc +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Fri Jun 7 22:38:30 2013 -0700 - Added documentation regarding the use of Ceres with cmake (once installed) - Commets about the flag ``BUILD_DOCUMENTATION=ON`` + Covariance estimation using SuiteSparseQR. - Change-Id: I8814927e60af190c8043bfc36e77fe76bfe6f562 + Change-Id: I70d1686e3288fdde5f9723e832e15ffb857d6d85 -commit f46de9e697eb5b8756084615e29ded48600a4d39 -Author: Sergey Sharybin <sergey.vfx@gmail.com> -Date: Thu Mar 21 15:31:35 2013 +0600 +commit 719889b8b7a3ef6712516d169a4ce3a33d272fda +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed Jul 17 11:31:08 2013 -0700 - Silent no previous declaration warning for FindParameterBlockOrDie + Minor fixes - Use anonymous namespace for this. Also move some surrounding static - function to this anonymous namespace. + 1. Typo in c_api.h + 2. The stream operator for FunctionSample is now in the ceres::internal namespace. - Change-Id: Ie235eb7936976563a9db115ec13c59e6e6869b96 + Change-Id: Id927a7a49c47d8903505535749ecca78cd2e83b3 -commit 16636efeffacdd69d075a60ea8a94d98fd81c6fd -Author: Sergey Sharybin <sergey.vfx@gmail.com> -Date: Thu Mar 21 15:12:01 2013 +0600 +commit 12cc164f79bb8a31e0eb3946e6f4898ac3c21c55 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Wed Jul 17 12:08:33 2013 +0100 - Compilation fix for msvc2010 + Minor fix to reject a line search config with negative L-BFGS rank. - Usage of back_inserter requires <iterator> header when using msvc2010 + Change-Id: Iad4c678efe574ef6696c34bd2a0ce61a504c7344 + +commit 9aa0e3cf7243a2e837bbfa22d4677010463f6a4e +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Fri Jul 5 20:22:37 2013 +0100 + + Adding Wolfe line search algorithm and full BFGS search direction options. - Change-Id: I92ee1649795ce0468ce337fc414eb0ca6e90c51e + Change-Id: I9d3fb117805bdfa5bc33613368f45ae8f10e0d79 -commit ac0d416991274ed67fe85371f09b07f706a6db9a -Author: Pablo Speciale <pablo.speciale@gmail.com> -Date: Wed Mar 20 18:32:14 2013 -0700 +commit 51c772c843ccecca006c706a9f64b8cbaf5416f9 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue Jul 16 16:42:52 2013 -0700 - google-glog link wasn't working, 'http://' twice + householderQR -> colPivHouseholderQR. - Change-Id: I9cd96d3609f9b1ba31cd480fef1702972be86741 + Change-Id: Ida623e853711f665e7a9d3b140a93e861591f96d -commit 55b6c966c4f697cb5d11982201733aa3bce7a5a7 -Author: Pablo Speciale <pablo.speciale@gmail.com> -Date: Wed Mar 20 17:44:04 2013 -0700 +commit c2c6411d16db95cde0cc3a7a80bac87266234bb7 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sat Jul 13 18:47:49 2013 -0700 - * Fixed the location of the Ceres doc (once installed with 'make install') - * Doing 'make ceres_docs' can be used to create the documentation (if the BUILD_DOCUMENTATION=ON) - * Included the copyright boilerplate for FindSphinx.cmake + DENSE_QR solver now uses non pivoting QR decomposition. - Change-Id: Iea21eba9e68384b4fe72c85fa88c76b0ba8a7a1d + Change-Id: I9099221448ccf71d0de20b9f652405009a6c24c5 -commit a986912555b304a47dd0c2a02892046fde15d091 +commit 3c2ad4018c8d2271434b9ff2bd05437b96f4927c Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Mar 20 11:50:34 2013 -0700 +Date: Mon Jul 15 08:09:38 2013 -0700 - Update version history + Speed up Automatic differentiation by 7%. - Change-Id: I238279719219a26d0d1bb32e0610f41007d3dcef + 1. Templatize Make1stOrderPerturbation. + 2. Convert a hard CHECK into DCHECK. + + Change-Id: I02cd67f2b87bc5722f1a090057d55f23e98d2c3b -commit 16dbf11626c52c013f1dca6375f993a554e31d51 -Author: Pablo Speciale <pablo.speciale@gmail.com> -Date: Mon Mar 11 14:44:02 2013 -0700 +commit 0a07fbf8731adcdce98c8e73127d379199341132 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed Jul 10 11:57:35 2013 -0700 - Added CeresConfig.cmake based on this example: - https://projects.kde.org/projects/kde/kdeexamples/repository/revisions/master/show/buildsystem/HowToInstallALibrary + Use ATLAS as the example BLAS in building.rst + + OpenBLAS has subtle issues releated to threading. It + conflicts with the use of threads in the other parts of + the application. - Change-Id: I130cac5d43d9fbbf359abc04d3691e25c4e2bb63 + Careful users can still use it by disabling threads via + an environment variable, but by default we want to use + a BLAS/LAPACK that does not suffer from these problems. + + Change-Id: I8c1c0ed0b526453564c5f9ea69b646fac32fe027 -commit 015d57f173fab7ea040ee01474101e208ff72be6 -Author: Pablo Speciale <pablo.speciale@gmail.com> -Date: Tue Mar 19 14:05:14 2013 -0700 +commit aee5597acf9c2c064977e937f52689254ebd1a39 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue Jul 9 23:30:07 2013 -0700 - Avoiding the Warning: "deprecated conversion from string constant to char*" + Minor fix to curve_fitting.c - Change-Id: Ifa47f9b0724f79c5c695828628c89818ddefd844 + Change-Id: Ib3669a5c4c73178b088dc1e80141f844f807b179 -commit c51b11c1046366035e7da95e4d8a78100ef3f153 -Author: Pablo Speciale <pablo.speciale@gmail.com> -Date: Tue Mar 12 00:56:56 2013 -0700 +commit bd82f82c3afeb3c57fa03f61fdbb0388f9ed8b02 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue Jul 9 23:19:09 2013 -0700 - Sphinx and CMake, based on this example: - http://ericscottbarr.com/blog/2012/03/sphinx-and-cmake-beautiful-documentation-for-c-projects/ + More CMake file cleanup. - The 'docs/CMakeLists.txt' file was deleted in this commit: 0abfb8f46f534b05413bb4d64b960d6fd0a9befb + Reduce the verbosity of the Cmake file. All the "Checking for" + messages have been removed since we log both success and failures. - Thanks to Arnaud Gelas, he has passed some links: - https://github.com/InsightSoftwareConsortium/ITKExamples/blob/master/CMake/FindSphinx.cmake - https://github.com/InsightSoftwareConsortium/ITKExamples/blob/master/CMakeLists.txt#L120-L154 + Further, UFConfig is only searched for if SuiteSparse_config cannot + be found. - Change-Id: Ic65e7f8ec5280d1e71a897a144417a21761c5553 + Change-Id: I601a6ffc808e566ff78ce232c86519ef413f0b33 -commit 793a339335d8d52279efb49bcd704d196646efb5 +commit 9f4552b6475616df7e60681e60cd5afebb45a4ea Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Mar 13 12:14:00 2013 -0700 +Date: Tue Jul 9 00:10:08 2013 -0700 - Make Android.mk play better with the external consraints + Stop CMake from trying to detect OpenMP when using Clang. - Change-Id: Ia0a1037d97c032a4ba1a9acbf4e04c192d12ee61 + Change-Id: Ie14c6466475b401ba35dbf13adc2e8701999c969 -commit 700d50d8074f0273b305fe6d9f795f1dcb988048 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Mar 12 16:12:42 2013 -0700 +commit 6e8bd501b25dc308df7b1a5eed16edfd8442002e +Author: Keir Mierle <mierle@gmail.com> +Date: Thu May 23 01:49:08 2013 -0700 - Lint cleanup from William Rucklidge + Extend the C API to support loss functions + + This extends the C API to support loss functions. Both + user-supplied cost functions as well as the stock Ceres cost + functions (Cauchy, Huber, etc) are supported. In addition, this + adds a simple unit test for the C API. + + Supporting loss functions required changing the signature of the + ceres_add_residual_block() function to also take a thunk for the + loss function. - Change-Id: Iacbf77246109f687708696eee7fb6144d23e7ec5 + Change-Id: Iefa58cf709adbb8f24588e5eb6aed9aef46b6d73 -commit 8140f0fc979f5728f37cfb68362f31e7e37b46bb +commit 1ab7fde626c3d3ac02664183f21fedd397785bea Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Mar 12 09:45:08 2013 -0700 +Date: Mon Jul 8 10:03:49 2013 -0700 - Modularize the build. + Update gmock and gtest to the latest svn versions. - 1. Add -DLINE_SEARCH_MINIMIZER to CMake to make the line search - minimizer optional. - 2. Better handling of -DSUITESPARSE/-DCXSPARSE in top level cmake - file. - 3. Disable code which will never be used if SuiteSparse and/or - CXSparse is not available. - 4. Update build docs. - 5. Update jni/Android.mk - 6. Minor lint cleanup from William Rucklidge. + This fixes a variety of mac/clang/c++11 issues. - Change-Id: If60460a858000df82faed7a6bb056dd2bfdde562 + Change-Id: I52e76d733cd53c9bb2fda125e51a6b58a90e41b3 -commit c59c1e44727c62d43523b672c1c132865cd25784 +commit eeedd2e191f5ce404453c735061ad13bd45b939b Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Mar 11 17:28:38 2013 -0700 +Date: Sun Jul 7 23:04:31 2013 -0700 - Propagate ifdefs correctly to improve build efficiency. + Rationalize some of the variable names in Solver::Options. - With -DRESTRICT_SCHUR_SPECIALIZATIONS, now the various - specializations are empty, decreasing build time and - reducing the size of the static library. + lm_max_diagonal -> max_lm_diagonal + lm_min_diagonal -> min_lm_diagonal + linear_solver_max_num_iterations -> max_linear_solver_iterations + linear_solver_min_num_iterations -> min_linear_solver_iterations - Change-Id: I8ec431279741a9a83516a4167c54a364c4608143 + This follows the pattern for the other parameters in Solver::Options + where, the max/min is the first word followed by the name of the + parameter. + + Change-Id: I0893610fceb6b7983fdb458a65522ba7079596a7 -commit 32874b861fc54b33aa4272e8c81bb001aa1e1e60 -Author: Yuliy Schwartzburg <syx818@gmail.com> -Date: Fri Mar 8 11:30:44 2013 +0100 +commit 7a8f79792467e56012d43b5f9aa7aefce14d5ee9 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed Jul 3 09:03:55 2013 -0700 - Fix CMake "LIB_SUFFIX" for non-linux installations + Lint fixes - Change-Id: Ieb8a2825a4378b388149e7934ecc7b96ba5a29fa + Change-Id: Ic453597488ef92723a81a224e7443e8f454b25da -commit 58b8c68f29c2c15edbc5f77102796df661020312 +commit 67ccb7379e7eab709480e227323ea48ea91e7ccc Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sat Mar 9 17:17:43 2013 -0800 +Date: Wed Jul 3 06:28:34 2013 -0700 - Clean up rotation.h + Fix broken build. - Change-Id: I3370c9883728cda068c9650a2c2a50641fd8299c + Change-Id: Ieb122bb96d5776f962fff6d6e9345dfc855bfed7 -commit 020d8e1e48f341f3b990ac449998d36aaca2771f +commit 4f010b2db02f22cee8243ed83a49e63a305dbb76 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Mar 6 16:19:26 2013 -0800 +Date: Mon Jul 1 08:01:01 2013 -0700 - Better error reporting in the modeling API. + Improve Summary::FullReport when line search is used. - More informative error when user passes an - unknown parameter block to Problem methods. + Disable reporting of preconditioner when direct factorization + is being used. - Change-Id: I517360e4b0b55814904ca3e664877d76ad3f59e8 + Change-Id: Id264d2292c5cab608724a6a8fab5d588db950468 -commit 5e7ce8a950cf5794c63817827ce66a3a4e66e7b6 +commit 09244015e304b0ebfb2f2399edd2d97e3b9dcd8f Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Mar 6 11:38:41 2013 -0800 +Date: Sun Jun 30 14:33:23 2013 -0700 - Fix Problem::Evaluate documentation + Expose line search parameters in Solver::Options. - Change-Id: I8c70a24743cff2d9cface99ef0f5d34c78f769c6 + Change-Id: Ifc52980976e7bac73c8164d80518a5a19db1b79d -commit 0a4f5f8f7428148f21183e743d091d2079406604 -Author: Taylor Braun-Jones <taylor@braun-jones.org> -Date: Wed Mar 6 00:00:32 2013 -0500 +commit 1c70ae9aa626e591cda987a970c240dd40d23a69 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun Jun 30 12:50:43 2013 -0700 - Fix operator() signature in several sections of the documentation + Fix Solver::Summary when line search is used. + + Also enable line search in bundle_adjuster. - Change-Id: I73f9d150a738f7b136fbc1f98fc60b0f306bd7f9 + Change-Id: Ic4343a4334b9f5a6fdeab38d4e3e1f6932bbc601 -commit 2c648dbc43025927301684fc82d95ccf6b6c21bc -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Mar 5 15:20:15 2013 -0800 +commit 70b06c89c7491d7749957c8454769bfcb0108a97 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sun Jun 30 18:49:56 2013 +0100 - Make examples independent of ceres internals. + Fix update of L-BFGS history buffers after they become full. - Change-Id: I6b6913e067a86fea713646218c8da1439d349d74 + Previously there was an assignment dimension mismatch in the + history update; thus, over time, the history would contain + (only) replicated copies of the (max_num_corrections_ -1)-th + update and the most recent update. + + Change-Id: I26203acf689686d41a5029c675ebbe001fe05d90 -commit e7148795c3f2ce1f6625a7c81545707a6cbde3eb +commit a427c877f968d951b3cdcb5f5298deaf84647830 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Mar 4 10:17:30 2013 -0800 +Date: Mon Jun 24 17:50:56 2013 -0700 - Fix a memory leak in CXSparse::SolveCholesky. - - Thanks to Alexander Mordvintsev for reporting this. + Lint cleanup. - Change-Id: I5c6be4d3d28f062e83a1ad41cb8089c19362a005 + Change-Id: Ie489f1ff182d99251ed8c0728cc6ea8e1c262ce0 diff --git a/extern/libmv/third_party/ceres/SConscript b/extern/libmv/third_party/ceres/SConscript index 6e490cfd5cf..a914135fddc 100644 --- a/extern/libmv/third_party/ceres/SConscript +++ b/extern/libmv/third_party/ceres/SConscript @@ -21,7 +21,7 @@ defs.append('CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {') defs.append('CERES_HASH_NAMESPACE_END=}}') defs.append('CERES_NO_SUITESPARSE') defs.append('CERES_NO_CXSPARSE') -defs.append('CERES_NO_PROTOCOL_BUFFERS') +defs.append('CERES_NO_LAPACK') defs.append('CERES_RESTRICT_SCHUR_SPECIALIZATION') defs.append('CERES_HAVE_RWLOCK') diff --git a/extern/libmv/third_party/ceres/bundle.sh b/extern/libmv/third_party/ceres/bundle.sh index 65af263f7c4..6ab348af118 100755 --- a/extern/libmv/third_party/ceres/bundle.sh +++ b/extern/libmv/third_party/ceres/bundle.sh @@ -160,7 +160,7 @@ add_definitions( -DCERES_HAVE_PTHREAD -DCERES_NO_SUITESPARSE -DCERES_NO_CXSPARSE - -DCERES_NO_PROTOCOL_BUFFERS + -DCERES_NO_LAPACK -DCERES_RESTRICT_SCHUR_SPECIALIZATION -DCERES_HAVE_RWLOCK ) @@ -218,7 +218,7 @@ defs.append('CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {') defs.append('CERES_HASH_NAMESPACE_END=}}') defs.append('CERES_NO_SUITESPARSE') defs.append('CERES_NO_CXSPARSE') -defs.append('CERES_NO_PROTOCOL_BUFFERS') +defs.append('CERES_NO_LAPACK') defs.append('CERES_RESTRICT_SCHUR_SPECIALIZATION') defs.append('CERES_HAVE_RWLOCK') diff --git a/extern/libmv/third_party/ceres/files.txt b/extern/libmv/third_party/ceres/files.txt index cb6c8af533e..071ccda655c 100644 --- a/extern/libmv/third_party/ceres/files.txt +++ b/extern/libmv/third_party/ceres/files.txt @@ -1,9 +1,11 @@ include/ceres/autodiff_cost_function.h include/ceres/autodiff_local_parameterization.h +include/ceres/c_api.h include/ceres/ceres.h include/ceres/conditioned_cost_function.h include/ceres/cost_function.h include/ceres/cost_function_to_functor.h +include/ceres/covariance.h include/ceres/crs_matrix.h include/ceres/dynamic_autodiff_cost_function.h include/ceres/fpclassify.h @@ -32,6 +34,7 @@ include/ceres/solver.h include/ceres/types.h internal/ceres/array_utils.cc internal/ceres/array_utils.h +internal/ceres/blas.cc internal/ceres/blas.h internal/ceres/block_evaluate_preparer.cc internal/ceres/block_evaluate_preparer.h @@ -39,6 +42,8 @@ internal/ceres/block_jacobian_writer.cc internal/ceres/block_jacobian_writer.h internal/ceres/block_jacobi_preconditioner.cc internal/ceres/block_jacobi_preconditioner.h +internal/ceres/block_random_access_crs_matrix.cc +internal/ceres/block_random_access_crs_matrix.h internal/ceres/block_random_access_dense_matrix.cc internal/ceres/block_random_access_dense_matrix.h internal/ceres/block_random_access_matrix.cc @@ -51,11 +56,14 @@ internal/ceres/block_structure.cc internal/ceres/block_structure.h internal/ceres/canonical_views_clustering.cc internal/ceres/canonical_views_clustering.h +internal/ceres/c_api.cc internal/ceres/casts.h internal/ceres/cgnr_linear_operator.h internal/ceres/cgnr_solver.cc internal/ceres/cgnr_solver.h internal/ceres/collections_port.h +internal/ceres/compressed_col_sparse_matrix_utils.cc +internal/ceres/compressed_col_sparse_matrix_utils.h internal/ceres/compressed_row_jacobian_writer.cc internal/ceres/compressed_row_jacobian_writer.h internal/ceres/compressed_row_sparse_matrix.cc @@ -67,6 +75,9 @@ internal/ceres/coordinate_descent_minimizer.cc internal/ceres/coordinate_descent_minimizer.h internal/ceres/corrector.cc internal/ceres/corrector.h +internal/ceres/covariance.cc +internal/ceres/covariance_impl.cc +internal/ceres/covariance_impl.h internal/ceres/cxsparse.cc internal/ceres/cxsparse.h internal/ceres/dense_jacobian_writer.h @@ -108,9 +119,13 @@ internal/ceres/graph_algorithms.h internal/ceres/graph.h internal/ceres/implicit_schur_complement.cc internal/ceres/implicit_schur_complement.h +internal/ceres/incomplete_lq_factorization.cc +internal/ceres/incomplete_lq_factorization.h internal/ceres/integral_types.h internal/ceres/iterative_schur_complement_solver.cc internal/ceres/iterative_schur_complement_solver.h +internal/ceres/lapack.cc +internal/ceres/lapack.h internal/ceres/levenberg_marquardt_strategy.cc internal/ceres/levenberg_marquardt_strategy.h internal/ceres/linear_least_squares_problems.cc @@ -130,7 +145,6 @@ internal/ceres/loss_function.cc internal/ceres/low_rank_inverse_hessian.cc internal/ceres/low_rank_inverse_hessian.h internal/ceres/map_util.h -internal/ceres/matrix_proto.h internal/ceres/minimizer.cc internal/ceres/minimizer.h internal/ceres/mutex.h @@ -166,6 +180,7 @@ internal/ceres/schur_jacobi_preconditioner.cc internal/ceres/schur_jacobi_preconditioner.h internal/ceres/scratch_evaluate_preparer.cc internal/ceres/scratch_evaluate_preparer.h +internal/ceres/small_blas.h internal/ceres/solver.cc internal/ceres/solver_impl.cc internal/ceres/solver_impl.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 e758d3a2bd5..371a11f71ec 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 @@ -40,8 +40,11 @@ // this is hidden, and you should write the function as if T were a scalar type // (e.g. a double-precision floating point number). // -// The function must write the computed value in the last argument (the only -// non-const one) and return true to indicate success. +// The function must write the computed value in the last argument +// (the only non-const one) and return true to indicate +// success. Please see cost_function.h for details on how the return +// value maybe used to impose simple constraints on the parameter +// block. // // For example, consider a scalar error e = k - x'y, where both x and y are // two-dimensional column vector parameters, the prime sign indicates @@ -125,11 +128,11 @@ #ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ #define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ -#include <glog/logging.h> #include "ceres/internal/autodiff.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/sized_cost_function.h" #include "ceres/types.h" +#include "glog/logging.h" namespace ceres { diff --git a/extern/libmv/third_party/ceres/include/ceres/autodiff_local_parameterization.h b/extern/libmv/third_party/ceres/include/ceres/autodiff_local_parameterization.h index 1099061bec8..0aae6c73acf 100644 --- a/extern/libmv/third_party/ceres/include/ceres/autodiff_local_parameterization.h +++ b/extern/libmv/third_party/ceres/include/ceres/autodiff_local_parameterization.h @@ -58,7 +58,7 @@ namespace ceres { // // For example, Quaternions have a three dimensional local // parameterization. It's plus operation can be implemented as (taken -// from interncal/ceres/auto_diff_local_parameterization_test.cc) +// from internal/ceres/auto_diff_local_parameterization_test.cc) // // struct QuaternionPlus { // template<typename T> diff --git a/extern/libmv/third_party/ceres/include/ceres/c_api.h b/extern/libmv/third_party/ceres/include/ceres/c_api.h new file mode 100644 index 00000000000..add68dea16c --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/c_api.h @@ -0,0 +1,141 @@ +/* Ceres Solver - A fast non-linear least squares minimizer + * Copyright 2013 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: mierle@gmail.com (Keir Mierle) + * + * A minimal C API for Ceres. Not all functionality is included. This API is + * not intended for clients of Ceres, but is instead intended for easing the + * process of binding Ceres to other languages. + * + * Currently this is a work in progress. + */ + +#ifndef CERES_PUBLIC_C_API_H_ +#define CERES_PUBLIC_C_API_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Init the Ceres private data. Must be called before anything else. */ +void ceres_init(); + +/* Equivalent to CostFunction::Evaluate() in the C++ API. + * + * The user may keep private information inside the opaque user_data object. + * The pointer here is the same one passed in the ceres_add_residual_block(). + */ +typedef int (*ceres_cost_function_t)(void* user_data, + double** parameters, + double* residuals, + double** jacobians); + +/* Equivalent to LossFunction::Evaluate() from the C++ API. */ +typedef void (*ceres_loss_function_t)(void* user_data, + double squared_norm, + double out[3]); + +/* Create callback data for Ceres' stock loss functions. + * + * Ceres has several loss functions available by default, and these functions + * expose those to the C API. To use the stock loss functions, call + * ceres_create_*_loss_data(), which internally creates an instance of one of + * the stock loss functions (for example ceres::CauchyLoss), and pass the + * returned "loss_function_data" along with the ceres_stock_loss_function to + * ceres_add_residual_block(). + * + * For example: + * + * void* cauchy_loss_function_data = + * ceres_create_cauchy_loss_function_data(1.2, 0.0); + * ceres_problem_add_residual_block( + * problem, + * my_cost_function, + * my_cost_function_data, + * ceres_stock_loss_function, + * cauchy_loss_function_data, + * 1, + * 2, + * parameter_sizes, + * parameter_pointers); + * ... + * ceres_free_stock_loss_function_data(cauchy_loss_function_data); + * + * See loss_function.h for the details of each loss function. + */ +void* ceres_create_huber_loss_function_data(double a); +void* ceres_create_softl1_loss_function_data(double a); +void* ceres_create_cauchy_loss_function_data(double a); +void* ceres_create_arctan_loss_function_data(double a); +void* ceres_create_tolerant_loss_function_data(double a, double b); + +/* Free the given stock loss function data. */ +void ceres_free_stock_loss_function_data(void* loss_function_data); + +/* This is an implementation of ceres_loss_function_t contained within Ceres + * itself, intended as a way to access the various stock Ceres loss functions + * from the C API. This should be passed to ceres_add_residual() below, in + * combination with a user_data pointer generated by + * ceres_create_stock_loss_function() above. */ +void ceres_stock_loss_function(void* user_data, + double squared_norm, + double out[3]); + +/* Equivalent to Problem from the C++ API. */ +struct ceres_problem_s; +typedef struct ceres_problem_s ceres_problem_t; + +struct ceres_residual_block_id_s; +typedef struct ceres_residual_block_id_s ceres_residual_block_id_t; + +/* Create and destroy a problem */ +/* TODO(keir): Add options for the problem. */ +ceres_problem_t* ceres_create_problem(); +void ceres_free_problem(ceres_problem_t* problem); + +/* Add a residual block. */ +ceres_residual_block_id_t* ceres_problem_add_residual_block( + ceres_problem_t* problem, + ceres_cost_function_t cost_function, + void* cost_function_data, + ceres_loss_function_t loss_function, + void* loss_function_data, + int num_residuals, + int num_parameter_blocks, + int* parameter_block_sizes, + double** parameters); + +void ceres_solve(ceres_problem_t* problem); + +/* TODO(keir): Figure out a way to pass a config in. */ + +#ifdef __cplusplus +} +#endif + +#endif /* CERES_PUBLIC_C_API_H_ */ diff --git a/extern/libmv/third_party/ceres/include/ceres/ceres.h b/extern/libmv/third_party/ceres/include/ceres/ceres.h index ac76e97c834..61b8b94dcaa 100644 --- a/extern/libmv/third_party/ceres/include/ceres/ceres.h +++ b/extern/libmv/third_party/ceres/include/ceres/ceres.h @@ -34,13 +34,14 @@ #ifndef CERES_PUBLIC_CERES_H_ #define CERES_PUBLIC_CERES_H_ -#define CERES_VERSION 1.5.0 -#define CERES_ABI_VERSION 1.5.0 +#define CERES_VERSION 1.7.0 +#define CERES_ABI_VERSION 1.7.0 #include "ceres/autodiff_cost_function.h" #include "ceres/autodiff_local_parameterization.h" #include "ceres/cost_function.h" #include "ceres/cost_function_to_functor.h" +#include "ceres/covariance.h" #include "ceres/crs_matrix.h" #include "ceres/iteration_callback.h" #include "ceres/jet.h" diff --git a/extern/libmv/third_party/ceres/include/ceres/cost_function.h b/extern/libmv/third_party/ceres/include/ceres/cost_function.h index 9b010f78f9d..8013e962616 100644 --- a/extern/libmv/third_party/ceres/include/ceres/cost_function.h +++ b/extern/libmv/third_party/ceres/include/ceres/cost_function.h @@ -93,6 +93,24 @@ class CostFunction { // the case when computing cost only. If jacobians[i] is NULL, then // the jacobian block corresponding to the i'th parameter block must // not to be returned. + // + // The return value indicates whether the computation of the + // residuals and/or jacobians was successful or not. + // + // This can be used to communicate numerical failures in jacobian + // computations for instance. + // + // A more interesting and common use is to impose constraints on the + // parameters. If the initial values of the parameter blocks satisfy + // the constraints, then returning false whenever the constraints + // are not satisfied will prevent the solver from moving into the + // infeasible region. This is not a very sophisticated mechanism for + // enforcing constraints, but is often good enough for things like + // non-negativity constraints. + // + // Note that it is important that the initial values of the + // parameter block must be feasible, otherwise the solver will + // declare a numerical problem at iteration 0. virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const = 0; diff --git a/extern/libmv/third_party/ceres/include/ceres/covariance.h b/extern/libmv/third_party/ceres/include/ceres/covariance.h new file mode 100644 index 00000000000..83126b5afef --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/covariance.h @@ -0,0 +1,422 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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_COVARIANCE_H_ +#define CERES_PUBLIC_COVARIANCE_H_ + +#include <utility> +#include <vector> +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +class Problem; + +namespace internal { +class CovarianceImpl; +} // namespace internal + +// WARNING +// ======= +// It is very easy to use this class incorrectly without understanding +// the underlying mathematics. Please read and understand the +// documentation completely before attempting to use this class. +// +// +// This class allows the user to evaluate the covariance for a +// non-linear least squares problem and provides random access to its +// blocks +// +// Background +// ========== +// One way to assess the quality of the solution returned by a +// non-linear least squares solve is to analyze the covariance of the +// solution. +// +// Let us consider the non-linear regression problem +// +// y = f(x) + N(0, I) +// +// i.e., the observation y is a random non-linear function of the +// independent variable x with mean f(x) and identity covariance. Then +// the maximum likelihood estimate of x given observations y is the +// solution to the non-linear least squares problem: +// +// x* = arg min_x |f(x)|^2 +// +// And the covariance of x* is given by +// +// C(x*) = inverse[J'(x*)J(x*)] +// +// Here J(x*) is the Jacobian of f at x*. The above formula assumes +// that J(x*) has full column rank. +// +// If J(x*) is rank deficient, then the covariance matrix C(x*) is +// also rank deficient and is given by +// +// C(x*) = pseudoinverse[J'(x*)J(x*)] +// +// Note that in the above, we assumed that the covariance +// matrix for y was identity. This is an important assumption. If this +// is not the case and we have +// +// y = f(x) + N(0, S) +// +// Where S is a positive semi-definite matrix denoting the covariance +// of y, then the maximum likelihood problem to be solved is +// +// x* = arg min_x f'(x) inverse[S] f(x) +// +// and the corresponding covariance estimate of x* is given by +// +// C(x*) = inverse[J'(x*) inverse[S] J(x*)] +// +// So, if it is the case that the observations being fitted to have a +// covariance matrix not equal to identity, then it is the user's +// responsibility that the corresponding cost functions are correctly +// scaled, e.g. in the above case the cost function for this problem +// should evaluate S^{-1/2} f(x) instead of just f(x), where S^{-1/2} +// is the inverse square root of the covariance matrix S. +// +// This class allows the user to evaluate the covariance for a +// non-linear least squares problem and provides random access to its +// blocks. The computation assumes that the CostFunctions compute +// residuals such that their covariance is identity. +// +// Since the computation of the covariance matrix requires computing +// the inverse of a potentially large matrix, this can involve a +// rather large amount of time and memory. However, it is usually the +// case that the user is only interested in a small part of the +// covariance matrix. Quite often just the block diagonal. This class +// allows the user to specify the parts of the covariance matrix that +// she is interested in and then uses this information to only compute +// and store those parts of the covariance matrix. +// +// Rank of the Jacobian +// -------------------- +// As we noted above, if the jacobian is rank deficient, then the +// inverse of J'J is not defined and instead a pseudo inverse needs to +// be computed. +// +// The rank deficiency in J can be structural -- columns which are +// always known to be zero or numerical -- depending on the exact +// values in the Jacobian. +// +// Structural rank deficiency occurs when the problem contains +// parameter blocks that are constant. This class correctly handles +// structural rank deficiency like that. +// +// Numerical rank deficiency, where the rank of the matrix cannot be +// predicted by its sparsity structure and requires looking at its +// numerical values is more complicated. Here again there are two +// cases. +// +// a. The rank deficiency arises from overparameterization. e.g., a +// four dimensional quaternion used to parameterize SO(3), which is +// a three dimensional manifold. In cases like this, the user should +// use an appropriate LocalParameterization. Not only will this lead +// to better numerical behaviour of the Solver, it will also expose +// the rank deficiency to the Covariance object so that it can +// handle it correctly. +// +// b. More general numerical rank deficiency in the Jacobian +// requires the computation of the so called Singular Value +// Decomposition (SVD) of J'J. We do not know how to do this for +// large sparse matrices efficiently. For small and moderate sized +// problems this is done using dense linear algebra. +// +// Gauge Invariance +// ---------------- +// In structure from motion (3D reconstruction) problems, the +// reconstruction is ambiguous upto a similarity transform. This is +// known as a Gauge Ambiguity. Handling Gauges correctly requires the +// use of SVD or custom inversion algorithms. For small problems the +// user can use the dense algorithm. For more details see +// +// Ken-ichi Kanatani, Daniel D. Morris: Gauges and gauge +// transformations for uncertainty description of geometric structure +// with indeterminacy. IEEE Transactions on Information Theory 47(5): +// 2017-2028 (2001) +// +// Example Usage +// ============= +// +// double x[3]; +// double y[2]; +// +// Problem problem; +// problem.AddParameterBlock(x, 3); +// problem.AddParameterBlock(y, 2); +// <Build Problem> +// <Solve Problem> +// +// Covariance::Options options; +// Covariance covariance(options); +// +// vector<pair<const double*, const double*> > covariance_blocks; +// covariance_blocks.push_back(make_pair(x, x)); +// covariance_blocks.push_back(make_pair(y, y)); +// covariance_blocks.push_back(make_pair(x, y)); +// +// CHECK(covariance.Compute(covariance_blocks, &problem)); +// +// double covariance_xx[3 * 3]; +// double covariance_yy[2 * 2]; +// double covariance_xy[3 * 2]; +// covariance.GetCovarianceBlock(x, x, covariance_xx) +// covariance.GetCovarianceBlock(y, y, covariance_yy) +// covariance.GetCovarianceBlock(x, y, covariance_xy) +// +class Covariance { + public: + struct Options { + Options() +#ifndef CERES_NO_SUITESPARSE + : algorithm_type(SPARSE_QR), +#else + : algorithm_type(DENSE_SVD), +#endif + min_reciprocal_condition_number(1e-14), + null_space_rank(0), + num_threads(1), + apply_loss_function(true) { + } + + // Ceres supports three different algorithms for covariance + // estimation, which represent different tradeoffs in speed, + // accuracy and reliability. + // + // 1. DENSE_SVD uses Eigen's JacobiSVD to perform the + // computations. It computes the singular value decomposition + // + // U * S * V' = J + // + // and then uses it to compute the pseudo inverse of J'J as + // + // pseudoinverse[J'J]^ = V * pseudoinverse[S] * V' + // + // It is an accurate but slow method and should only be used + // for small to moderate sized problems. It can handle + // full-rank as well as rank deficient Jacobians. + // + // 2. SPARSE_CHOLESKY uses the CHOLMOD sparse Cholesky + // factorization library to compute the decomposition : + // + // R'R = J'J + // + // and then + // + // [J'J]^-1 = [R'R]^-1 + // + // It a fast algorithm for sparse matrices that should be used + // when the Jacobian matrix J is well conditioned. For + // ill-conditioned matrices, this algorithm can fail + // unpredictabily. This is because Cholesky factorization is + // not a rank-revealing factorization, i.e., it cannot reliably + // detect when the matrix being factorized is not of full + // rank. SuiteSparse/CHOLMOD supplies a heuristic for checking + // if the matrix is rank deficient (cholmod_rcond), but it is + // only a heuristic and can have both false positive and false + // negatives. + // + // Recent versions of SuiteSparse (>= 4.2.0) provide a much + // more efficient method for solving for rows of the covariance + // matrix. Therefore, if you are doing SPARSE_CHOLESKY, we + // strongly recommend using a recent version of SuiteSparse. + // + // 3. SPARSE_QR uses the SuiteSparseQR sparse QR factorization + // library to compute the decomposition + // + // Q * R = J + // + // [J'J]^-1 = [R*R']^-1 + // + // It is a moderately fast algorithm for sparse matrices, which + // at the price of more time and memory than the + // SPARSE_CHOLESKY algorithm is numerically better behaved and + // is rank revealing, i.e., it can reliably detect when the + // Jacobian matrix is rank deficient. + // + // Neither SPARSE_CHOLESKY or SPARSE_QR are capable of computing + // the covariance if the Jacobian is rank deficient. + + CovarianceAlgorithmType algorithm_type; + + // If the Jacobian matrix is near singular, then inverting J'J + // will result in unreliable results, e.g, if + // + // J = [1.0 1.0 ] + // [1.0 1.0000001 ] + // + // which is essentially a rank deficient matrix, we have + // + // inv(J'J) = [ 2.0471e+14 -2.0471e+14] + // [-2.0471e+14 2.0471e+14] + // + // This is not a useful result. Therefore, by default + // Covariance::Compute will return false if a rank deficient + // Jacobian is encountered. How rank deficiency is detected + // depends on the algorithm being used. + // + // 1. DENSE_SVD + // + // min_sigma / max_sigma < sqrt(min_reciprocal_condition_number) + // + // where min_sigma and max_sigma are the minimum and maxiumum + // singular values of J respectively. + // + // 2. SPARSE_CHOLESKY + // + // cholmod_rcond < min_reciprocal_conditioner_number + // + // Here cholmod_rcond is a crude estimate of the reciprocal + // condition number of J'J by using the maximum and minimum + // diagonal entries of the Cholesky factor R. There are no + // theoretical guarantees associated with this test. It can + // give false positives and negatives. Use at your own + // risk. The default value of min_reciprocal_condition_number + // has been set to a conservative value, and sometimes the + // Covariance::Compute may return false even if it is possible + // to estimate the covariance reliably. In such cases, the user + // should exercise their judgement before lowering the value of + // min_reciprocal_condition_number. + // + // 3. SPARSE_QR + // + // rank(J) < num_col(J) + // + // Here rank(J) is the estimate of the rank of J returned by the + // SuiteSparseQR algorithm. It is a fairly reliable indication + // of rank deficiency. + // + double min_reciprocal_condition_number; + + // When using DENSE_SVD, the user has more control in dealing with + // singular and near singular covariance matrices. + // + // As mentioned above, when the covariance matrix is near + // singular, instead of computing the inverse of J'J, the + // Moore-Penrose pseudoinverse of J'J should be computed. + // + // If J'J has the eigen decomposition (lambda_i, e_i), where + // lambda_i is the i^th eigenvalue and e_i is the corresponding + // eigenvector, then the inverse of J'J is + // + // inverse[J'J] = sum_i e_i e_i' / lambda_i + // + // and computing the pseudo inverse involves dropping terms from + // this sum that correspond to small eigenvalues. + // + // How terms are dropped is controlled by + // min_reciprocal_condition_number and null_space_rank. + // + // If null_space_rank is non-negative, then the smallest + // null_space_rank eigenvalue/eigenvectors are dropped + // irrespective of the magnitude of lambda_i. If the ratio of the + // smallest non-zero eigenvalue to the largest eigenvalue in the + // truncated matrix is still below + // min_reciprocal_condition_number, then the Covariance::Compute() + // will fail and return false. + // + // Setting null_space_rank = -1 drops all terms for which + // + // lambda_i / lambda_max < min_reciprocal_condition_number. + // + // This option has no effect on the SPARSE_CHOLESKY or SPARSE_QR + // algorithms. + int null_space_rank; + + int num_threads; + + // Even though the residual blocks in the problem may contain loss + // functions, setting apply_loss_function to false will turn off + // the application of the loss function to the output of the cost + // function and in turn its effect on the covariance. + // + // TODO(sameergaarwal): Expand this based on Jim's experiments. + bool apply_loss_function; + }; + + explicit Covariance(const Options& options); + ~Covariance(); + + // Compute a part of the covariance matrix. + // + // The vector covariance_blocks, indexes into the covariance matrix + // block-wise using pairs of parameter blocks. This allows the + // covariance estimation algorithm to only compute and store these + // blocks. + // + // Since the covariance matrix is symmetric, if the user passes + // (block1, block2), then GetCovarianceBlock can be called with + // block1, block2 as well as block2, block1. + // + // covariance_blocks cannot contain duplicates. Bad things will + // happen if they do. + // + // Note that the list of covariance_blocks is only used to determine + // what parts of the covariance matrix are computed. The full + // Jacobian is used to do the computation, i.e. they do not have an + // impact on what part of the Jacobian is used for computation. + // + // The return value indicates the success or failure of the + // covariance computation. Please see the documentation for + // Covariance::Options for more on the conditions under which this + // function returns false. + bool Compute( + const vector<pair<const double*, const double*> >& covariance_blocks, + Problem* problem); + + // Return the block of the covariance matrix corresponding to + // parameter_block1 and parameter_block2. + // + // Compute must be called before the first call to + // GetCovarianceBlock and the pair <parameter_block1, + // parameter_block2> OR the pair <parameter_block2, + // parameter_block1> must have been present in the vector + // covariance_blocks when Compute was called. Otherwise + // GetCovarianceBlock will return false. + // + // covariance_block must point to a memory location that can store a + // parameter_block1_size x parameter_block2_size matrix. The + // returned covariance will be a row-major matrix. + bool GetCovarianceBlock(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const; + + private: + internal::scoped_ptr<internal::CovarianceImpl> impl_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_COVARIANCE_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/dynamic_autodiff_cost_function.h b/extern/libmv/third_party/ceres/include/ceres/dynamic_autodiff_cost_function.h index 38bdb0aa618..5d8f188e5a7 100644 --- a/extern/libmv/third_party/ceres/include/ceres/dynamic_autodiff_cost_function.h +++ b/extern/libmv/third_party/ceres/include/ceres/dynamic_autodiff_cost_function.h @@ -126,17 +126,28 @@ class DynamicAutoDiffCostFunction : public CostFunction { vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks, static_cast<Jet<double, Stride>* >(NULL)); int num_active_parameters = 0; - int start_derivative_section = -1; - for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) { + + // To handle constant parameters between non-constant parameter blocks, the + // start position --- a raw parameter index --- of each contiguous block of + // non-constant parameters is recorded in start_derivative_section. + vector<int> start_derivative_section; + bool in_derivative_section = false; + int parameter_cursor = 0; + + // Discover the derivative sections and set the parameter values. + for (int i = 0; i < num_parameter_blocks; ++i) { jet_parameters[i] = &input_jets[parameter_cursor]; const int parameter_block_size = parameter_block_sizes()[i]; if (jacobians[i] != NULL) { - start_derivative_section = - (start_derivative_section == -1) - ? parameter_cursor - : start_derivative_section; + if (!in_derivative_section) { + start_derivative_section.push_back(parameter_cursor); + in_derivative_section = true; + } + num_active_parameters += parameter_block_size; + } else { + in_derivative_section = false; } for (int j = 0; j < parameter_block_size; ++j, parameter_cursor++) { @@ -144,29 +155,54 @@ class DynamicAutoDiffCostFunction : public CostFunction { } } + // When `num_active_parameters % Stride != 0` then it can be the case + // that `active_parameter_count < Stride` while parameter_cursor is less + // than the total number of parameters and with no remaining non-constant + // parameter blocks. Pushing parameter_cursor (the total number of + // parameters) as a final entry to start_derivative_section is required + // because if a constant parameter block is encountered after the + // last non-constant block then current_derivative_section is incremented + // and would otherwise index an invalid position in + // start_derivative_section. Setting the final element to the total number + // of parameters means that this can only happen at most once in the loop + // below. + start_derivative_section.push_back(parameter_cursor); + // Evaluate all of the strides. Each stride is a chunk of the derivative to // evaluate, typically some size proportional to the size of the SIMD // registers of the CPU. int num_strides = static_cast<int>(ceil(num_active_parameters / static_cast<float>(Stride))); + int current_derivative_section = 0; + int current_derivative_section_cursor = 0; + for (int pass = 0; pass < num_strides; ++pass) { // Set most of the jet components to zero, except for // non-constant #Stride parameters. + const int initial_derivative_section = current_derivative_section; + const int initial_derivative_section_cursor = + current_derivative_section_cursor; + int active_parameter_count = 0; - int end_derivative_section = start_derivative_section; - for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) { + parameter_cursor = 0; + + for (int i = 0; i < num_parameter_blocks; ++i) { for (int j = 0; j < parameter_block_sizes()[i]; ++j, parameter_cursor++) { input_jets[parameter_cursor].v.setZero(); - if (parameter_cursor >= start_derivative_section && - active_parameter_count < Stride) { + if (active_parameter_count < Stride && + parameter_cursor >= ( + start_derivative_section[current_derivative_section] + + current_derivative_section_cursor)) { if (jacobians[i] != NULL) { - input_jets[parameter_cursor] - .v[parameter_cursor - start_derivative_section] = 1.0; + input_jets[parameter_cursor].v[active_parameter_count] = 1.0; ++active_parameter_count; + ++current_derivative_section_cursor; + } else { + ++current_derivative_section; + current_derivative_section_cursor = 0; } - ++end_derivative_section; } } } @@ -177,18 +213,27 @@ class DynamicAutoDiffCostFunction : public CostFunction { // Copy the pieces of the jacobians into their final place. active_parameter_count = 0; + + current_derivative_section = initial_derivative_section; + current_derivative_section_cursor = initial_derivative_section_cursor; + for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) { for (int j = 0; j < parameter_block_sizes()[i]; ++j, parameter_cursor++) { - if (parameter_cursor >= start_derivative_section && - active_parameter_count < Stride) { + if (active_parameter_count < Stride && + parameter_cursor >= ( + start_derivative_section[current_derivative_section] + + current_derivative_section_cursor)) { if (jacobians[i] != NULL) { for (int k = 0; k < num_residuals(); ++k) { jacobians[i][k * parameter_block_sizes()[i] + j] = - output_jets[k].v[parameter_cursor - - start_derivative_section]; + output_jets[k].v[active_parameter_count]; } ++active_parameter_count; + ++current_derivative_section_cursor; + } else { + ++current_derivative_section; + current_derivative_section_cursor = 0; } } } @@ -201,8 +246,6 @@ class DynamicAutoDiffCostFunction : public CostFunction { residuals[k] = output_jets[k].a; } } - - start_derivative_section = end_derivative_section; } return true; } diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/autodiff.h b/extern/libmv/third_party/ceres/include/ceres/internal/autodiff.h index 2b32671c06d..cf21d7a5001 100644 --- a/extern/libmv/third_party/ceres/include/ceres/internal/autodiff.h +++ b/extern/libmv/third_party/ceres/include/ceres/internal/autodiff.h @@ -142,11 +142,11 @@ #include <stddef.h> -#include <glog/logging.h> #include "ceres/jet.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" #include "ceres/internal/variadic_evaluate.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -165,13 +165,14 @@ namespace internal { // // is what would get put in dst if N was 3, offset was 3, and the jet type JetT // was 8-dimensional. -template <typename JetT, typename T> -inline void Make1stOrderPerturbation(int offset, int N, const T *src, - JetT *dst) { +template <typename JetT, typename T, int N> +inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) { DCHECK(src); DCHECK(dst); for (int j = 0; j < N; ++j) { - dst[j] = JetT(src[j], offset + j); + dst[j].a = src[j]; + dst[j].v.setZero(); + dst[j].v[offset + j] = 1.0; } } @@ -212,7 +213,7 @@ struct AutoDiff { T **jacobians) { // This block breaks the 80 column rule to keep it somewhat readable. DCHECK_GT(num_outputs, 0); - CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || @@ -258,12 +259,12 @@ struct AutoDiff { JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; -#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ - if (N ## i) { \ - internal::Make1stOrderPerturbation(jet ## i, \ - N ## i, \ - parameters[i], \ - x.get() + jet ## i); \ +#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ + if (N ## i) { \ + internal::Make1stOrderPerturbation<JetT, T, N ## i>( \ + jet ## i, \ + parameters[i], \ + x.get() + jet ## i); \ } CERES_MAKE_1ST_ORDER_PERTURBATION(0); CERES_MAKE_1ST_ORDER_PERTURBATION(1); 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 fa4a339d757..ee264d1619d 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 @@ -33,10 +33,10 @@ #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ #include <cstddef> -#include <glog/logging.h> #include "Eigen/Core" #include "ceres/internal/macros.h" #include "ceres/internal/manual_constructor.h" +#include "glog/logging.h" namespace ceres { namespace internal { diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/variadic_evaluate.h b/extern/libmv/third_party/ceres/include/ceres/internal/variadic_evaluate.h index 4b1e4bdc65a..9a473d5b25c 100644 --- a/extern/libmv/third_party/ceres/include/ceres/internal/variadic_evaluate.h +++ b/extern/libmv/third_party/ceres/include/ceres/internal/variadic_evaluate.h @@ -34,10 +34,10 @@ #include <stddef.h> -#include <glog/logging.h> #include "ceres/jet.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" +#include "glog/logging.h" namespace ceres { namespace internal { 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 0dc4c96b441..987c2d91f79 100644 --- a/extern/libmv/third_party/ceres/include/ceres/iteration_callback.h +++ b/extern/libmv/third_party/ceres/include/ceres/iteration_callback.h @@ -54,6 +54,8 @@ struct IterationSummary { eta(0.0), step_size(0.0), line_search_function_evaluations(0), + line_search_gradient_evaluations(0), + line_search_iterations(0), linear_solver_iterations(0), iteration_time_in_seconds(0.0), step_solver_time_in_seconds(0.0), @@ -121,13 +123,21 @@ struct IterationSummary { // Step sized computed by the line search algorithm. double step_size; - // Number of function evaluations used by the line search algorithm. + // Number of function value evaluations used by the line search algorithm. int line_search_function_evaluations; + // Number of function gradient evaluations used by the line search algorithm. + int line_search_gradient_evaluations; + + // Number of iterations taken by the line search algorithm. + int line_search_iterations; + // Number of iterations taken by the linear solver to solve for the // Newton step. int linear_solver_iterations; + // All times reported below are wall times. + // Time (in seconds) spent inside the minimizer loop in the current // iteration. double iteration_time_in_seconds; diff --git a/extern/libmv/third_party/ceres/include/ceres/jet.h b/extern/libmv/third_party/ceres/include/ceres/jet.h index 000bd1c116a..4d2a857dc3d 100644 --- a/extern/libmv/third_party/ceres/include/ceres/jet.h +++ b/extern/libmv/third_party/ceres/include/ceres/jet.h @@ -405,7 +405,6 @@ CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT // double-valued and Jet-valued functions, but we are not allowed to put // Jet-valued functions inside namespace std. // -// Missing: cosh, sinh, tanh, tan // TODO(keir): Switch to "using". inline double abs (double x) { return std::abs(x); } inline double log (double x) { return std::log(x); } @@ -415,6 +414,11 @@ 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 double tan (double x) { return std::tan(x); } +inline double atan (double x) { return std::atan(x); } +inline double sinh (double x) { return std::sinh(x); } +inline double cosh (double x) { return std::cosh(x); } +inline double tanh (double x) { return std::tanh(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); } @@ -495,6 +499,58 @@ Jet<T, N> asin(const Jet<T, N>& f) { return g; } +// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h +template <typename T, int N> inline +Jet<T, N> tan(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = tan(f.a); + double tan_a = tan(f.a); + const T tmp = T(1.0) + tan_a * tan_a; + g.v = tmp * f.v; + return g; +} + +// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h +template <typename T, int N> inline +Jet<T, N> atan(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = atan(f.a); + const T tmp = T(1.0) / (T(1.0) + f.a * f.a); + g.v = tmp * f.v; + return g; +} + +// sinh(a + h) ~= sinh(a) + cosh(a) h +template <typename T, int N> inline +Jet<T, N> sinh(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = sinh(f.a); + const T cosh_a = cosh(f.a); + g.v = cosh_a * f.v; + return g; +} + +// cosh(a + h) ~= cosh(a) + sinh(a) h +template <typename T, int N> inline +Jet<T, N> cosh(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = cosh(f.a); + const T sinh_a = sinh(f.a); + g.v = sinh_a * f.v; + return g; +} + +// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h +template <typename T, int N> inline +Jet<T, N> tanh(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = tanh(f.a); + double tanh_a = tanh(f.a); + const T tmp = T(1.0) - tanh_a * tanh_a; + g.v = tmp * f.v; + return g; +} + // Jet Classification. It is not clear what the appropriate semantics are for // these classifications. This picks that IsFinite and isnormal are "all" // operations, i.e. all elements of the jet must be finite for the jet itself @@ -645,6 +701,11 @@ template<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) template<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT template<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT template<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_tan (const Jet<T, N>& x) { return tan(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_atan(const Jet<T, N>& x) { return atan(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_sinh(const Jet<T, N>& x) { return sinh(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_cosh(const Jet<T, N>& x) { return cosh(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_tanh(const Jet<T, N>& x) { return tanh(x); } // NOLINT template<typename T, int N> inline Jet<T, N> ei_pow (const Jet<T, N>& x, Jet<T, N> y) { return pow(x, y); } // NOLINT // Note: This has to be in the ceres namespace for argument dependent lookup to 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 0c0ceaaecd0..b99c184525e 100644 --- a/extern/libmv/third_party/ceres/include/ceres/loss_function.h +++ b/extern/libmv/third_party/ceres/include/ceres/loss_function.h @@ -75,10 +75,10 @@ #ifndef CERES_PUBLIC_LOSS_FUNCTION_H_ #define CERES_PUBLIC_LOSS_FUNCTION_H_ -#include <glog/logging.h> #include "ceres/internal/macros.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" +#include "glog/logging.h" namespace ceres { @@ -347,19 +347,20 @@ class ScaledLoss : public LossFunction { // // CostFunction* cost_function = // new AutoDiffCostFunction < UW_Camera_Mapper, 2, 9, 3>( -// new UW_Camera_Mapper(data->observations[2*i + 0], -// data->observations[2*i + 1])); +// new UW_Camera_Mapper(feature_x, feature_y)); // // LossFunctionWrapper* loss_function(new HuberLoss(1.0), TAKE_OWNERSHIP); // // problem.AddResidualBlock(cost_function, loss_function, parameters); // // Solver::Options options; -// scoped_ptr<Solver::Summary> summary1(Solve(problem, options)); +// Solger::Summary summary; +// +// Solve(options, &problem, &summary) // // loss_function->Reset(new HuberLoss(1.0), TAKE_OWNERSHIP); // -// scoped_ptr<Solver::Summary> summary2(Solve(problem, options)); +// Solve(options, &problem, &summary) // class LossFunctionWrapper : public LossFunction { public: 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 555bc3d073f..a47a66d9672 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 @@ -36,8 +36,10 @@ // To get an numerically differentiated cost function, you must define // a class with a operator() (a functor) that computes the residuals. // -// The function must write the computed value in the last argument (the only -// non-const one) and return true to indicate success. +// The function must write the computed value in the last argument +// (the only non-const one) and return true to indicate success. +// Please see cost_function.h for details on how the return value +// maybe used to impose simple constraints on the parameter block. // // For example, consider a scalar error e = k - x'y, where both x and y are // two-dimensional column vector parameters, the prime sign indicates @@ -80,14 +82,14 @@ // // CostFunction* cost_function // = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, 1, 2, 2>( -// new MyScalarCostFunctor(1.0)); ^ ^ ^ -// | | | | -// Finite Differencing Scheme -+ | | | -// Dimension of residual ----------+ | | -// Dimension of x --------------------+ | -// Dimension of y -----------------------+ +// new MyScalarCostFunctor(1.0)); ^ ^ ^ ^ +// | | | | +// Finite Differencing Scheme -+ | | | +// Dimension of residual ------------+ | | +// Dimension of x ----------------------+ | +// Dimension of y -------------------------+ // -// In this example, there is usually an instance for each measumerent of k. +// In this example, there is usually an instance for each measurement of k. // // In the instantiation above, the template parameters following // "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing @@ -124,7 +126,7 @@ // To get a numerically differentiated cost function, define a // subclass of CostFunction such that the Evaluate() function ignores // the jacobian parameter. The numeric differentiation wrapper will -// fill in the jacobian parameter if nececssary by repeatedly calling +// fill in the jacobian parameter if necessary by repeatedly calling // the Evaluate() function with small changes to the appropriate // parameters, and computing the slope. For performance, the numeric // differentiation wrapper class is templated on the concrete cost @@ -146,13 +148,13 @@ #ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ #define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ -#include <glog/logging.h> #include "Eigen/Dense" #include "ceres/cost_function.h" #include "ceres/internal/numeric_diff.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/sized_cost_function.h" #include "ceres/types.h" +#include "glog/logging.h" namespace ceres { @@ -230,8 +232,8 @@ class NumericDiffCostFunction if (N5) parameters_reference_copy[5] = parameters_reference_copy[4] + N4; if (N6) parameters_reference_copy[6] = parameters_reference_copy[5] + N5; if (N7) parameters_reference_copy[7] = parameters_reference_copy[6] + N6; - if (N7) parameters_reference_copy[8] = parameters_reference_copy[7] + N7; - if (N8) parameters_reference_copy[9] = parameters_reference_copy[8] + N8; + if (N8) parameters_reference_copy[8] = parameters_reference_copy[7] + N7; + if (N9) parameters_reference_copy[9] = parameters_reference_copy[8] + N8; #define COPY_PARAMETER_BLOCK(block) \ if (N ## block) memcpy(parameters_reference_copy[block], \ diff --git a/extern/libmv/third_party/ceres/include/ceres/problem.h b/extern/libmv/third_party/ceres/include/ceres/problem.h index 33394ce0e17..663616ddb3b 100644 --- a/extern/libmv/third_party/ceres/include/ceres/problem.h +++ b/extern/libmv/third_party/ceres/include/ceres/problem.h @@ -329,12 +329,12 @@ class Problem { int NumResiduals() const; // The size of the parameter block. - int ParameterBlockSize(double* values) const; + int ParameterBlockSize(const double* values) const; // The size of local parameterization for the parameter block. If // there is no local parameterization associated with this parameter // block, then ParameterBlockLocalSize = ParameterBlockSize. - int ParameterBlockLocalSize(double* values) const; + int ParameterBlockLocalSize(const double* values) const; // Fills the passed parameter_blocks vector with pointers to the // parameter blocks currently in the problem. After this call, @@ -423,6 +423,7 @@ class Problem { private: friend class Solver; + friend class Covariance; internal::scoped_ptr<internal::ProblemImpl> problem_impl_; CERES_DISALLOW_COPY_AND_ASSIGN(Problem); }; diff --git a/extern/libmv/third_party/ceres/include/ceres/sized_cost_function.h b/extern/libmv/third_party/ceres/include/ceres/sized_cost_function.h index 6bfc1af31a2..4f98d4eb95c 100644 --- a/extern/libmv/third_party/ceres/include/ceres/sized_cost_function.h +++ b/extern/libmv/third_party/ceres/include/ceres/sized_cost_function.h @@ -38,9 +38,9 @@ #ifndef CERES_PUBLIC_SIZED_COST_FUNCTION_H_ #define CERES_PUBLIC_SIZED_COST_FUNCTION_H_ -#include <glog/logging.h> #include "ceres/types.h" #include "ceres/cost_function.h" +#include "glog/logging.h" namespace ceres { diff --git a/extern/libmv/third_party/ceres/include/ceres/solver.h b/extern/libmv/third_party/ceres/include/ceres/solver.h index 97d082d80e0..25b762a7bd5 100644 --- a/extern/libmv/third_party/ceres/include/ceres/solver.h +++ b/extern/libmv/third_party/ceres/include/ceres/solver.h @@ -60,9 +60,19 @@ class Solver { Options() { minimizer_type = TRUST_REGION; line_search_direction_type = LBFGS; - line_search_type = ARMIJO; + line_search_type = WOLFE; nonlinear_conjugate_gradient_type = FLETCHER_REEVES; max_lbfgs_rank = 20; + use_approximate_eigenvalue_bfgs_scaling = false; + line_search_interpolation_type = CUBIC; + min_line_search_step_size = 1e-9; + line_search_sufficient_function_decrease = 1e-4; + max_line_search_step_contraction = 1e-3; + min_line_search_step_contraction = 0.6; + max_num_line_search_step_size_iterations = 20; + max_num_line_search_direction_restarts = 5; + line_search_sufficient_curvature_decrease = 0.9; + max_line_search_step_expansion = 10.0; trust_region_strategy_type = LEVENBERG_MARQUARDT; dogleg_type = TRADITIONAL_DOGLEG; use_nonmonotonic_steps = false; @@ -74,8 +84,8 @@ class Solver { 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; + min_lm_diagonal = 1e-6; + max_lm_diagonal = 1e32; max_num_consecutive_invalid_steps = 5; function_tolerance = 1e-6; gradient_tolerance = 1e-10; @@ -89,24 +99,27 @@ class Solver { preconditioner_type = JACOBI; - sparse_linear_algebra_library = SUITE_SPARSE; + dense_linear_algebra_library_type = EIGEN; + sparse_linear_algebra_library_type = SUITE_SPARSE; #if defined(CERES_NO_SUITESPARSE) && !defined(CERES_NO_CXSPARSE) - sparse_linear_algebra_library = CX_SPARSE; + sparse_linear_algebra_library_type = CX_SPARSE; #endif + num_linear_solver_threads = 1; linear_solver_ordering = NULL; use_postordering = false; - use_inner_iterations = false; - inner_iteration_ordering = NULL; - linear_solver_min_num_iterations = 1; - linear_solver_max_num_iterations = 500; + min_linear_solver_iterations = 1; + max_linear_solver_iterations = 500; eta = 1e-1; jacobi_scaling = true; + use_inner_iterations = false; + inner_iteration_tolerance = 1e-3; + inner_iteration_ordering = NULL; logging_type = PER_MINIMIZER_ITERATION; minimizer_progress_to_stdout = false; - lsqp_dump_directory = "/tmp"; - lsqp_dump_format_type = TEXTFILE; + trust_region_problem_dump_directory = "/tmp"; + trust_region_problem_dump_format_type = TEXTFILE; check_gradients = false; gradient_check_relative_precision = 1e-8; numeric_derivative_relative_step_size = 1e-6; @@ -171,6 +184,109 @@ class Solver { // Limited Storage". Mathematics of Computation 35 (151): 773–782. int max_lbfgs_rank; + // As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS), + // the initial inverse Hessian approximation is taken to be the Identity. + // However, Oren showed that using instead I * \gamma, where \gamma is + // chosen to approximate an eigenvalue of the true inverse Hessian can + // result in improved convergence in a wide variety of cases. Setting + // use_approximate_eigenvalue_bfgs_scaling to true enables this scaling. + // + // It is important to note that approximate eigenvalue scaling does not + // always improve convergence, and that it can in fact significantly degrade + // performance for certain classes of problem, which is why it is disabled + // by default. In particular it can degrade performance when the + // sensitivity of the problem to different parameters varies significantly, + // as in this case a single scalar factor fails to capture this variation + // and detrimentally downscales parts of the jacobian approximation which + // correspond to low-sensitivity parameters. It can also reduce the + // robustness of the solution to errors in the jacobians. + // + // Oren S.S., Self-scaling variable metric (SSVM) algorithms + // Part II: Implementation and experiments, Management Science, + // 20(5), 863-874, 1974. + bool use_approximate_eigenvalue_bfgs_scaling; + + // Degree of the polynomial used to approximate the objective + // function. Valid values are BISECTION, QUADRATIC and CUBIC. + // + // BISECTION corresponds to pure backtracking search with no + // interpolation. + LineSearchInterpolationType line_search_interpolation_type; + + // If during the line search, the step_size falls below this + // value, it is truncated to zero. + double min_line_search_step_size; + + // Line search parameters. + + // Solving the line search problem exactly is computationally + // prohibitive. Fortunately, line search based optimization + // algorithms can still guarantee convergence if instead of an + // exact solution, the line search algorithm returns a solution + // which decreases the value of the objective function + // sufficiently. More precisely, we are looking for a step_size + // s.t. + // + // f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size + // + double line_search_sufficient_function_decrease; + + // In each iteration of the line search, + // + // new_step_size >= max_line_search_step_contraction * step_size + // + // Note that by definition, for contraction: + // + // 0 < max_step_contraction < min_step_contraction < 1 + // + double max_line_search_step_contraction; + + // In each iteration of the line search, + // + // new_step_size <= min_line_search_step_contraction * step_size + // + // Note that by definition, for contraction: + // + // 0 < max_step_contraction < min_step_contraction < 1 + // + double min_line_search_step_contraction; + + // Maximum number of trial step size iterations during each line search, + // if a step size satisfying the search conditions cannot be found within + // this number of trials, the line search will terminate. + int max_num_line_search_step_size_iterations; + + // Maximum number of restarts of the line search direction algorithm before + // terminating the optimization. Restarts of the line search direction + // algorithm occur when the current algorithm fails to produce a new descent + // direction. This typically indicates a numerical failure, or a breakdown + // in the validity of the approximations used. + int max_num_line_search_direction_restarts; + + // The strong Wolfe conditions consist of the Armijo sufficient + // decrease condition, and an additional requirement that the + // step-size be chosen s.t. the _magnitude_ ('strong' Wolfe + // conditions) of the gradient along the search direction + // decreases sufficiently. Precisely, this second condition + // is that we seek a step_size s.t. + // + // |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)| + // + // Where f() is the line search objective and f'() is the derivative + // of f w.r.t step_size (d f / d step_size). + double line_search_sufficient_curvature_decrease; + + // During the bracketing phase of the Wolfe search, the step size is + // increased until either a point satisfying the Wolfe conditions is + // found, or an upper bound for a bracket containing a point satisfying + // the conditions is found. Precisely, at each iteration of the + // expansion: + // + // new_step_size <= max_step_expansion * step_size. + // + // By definition for expansion, max_step_expansion > 1.0. + double max_line_search_step_expansion; + TrustRegionStrategyType trust_region_strategy_type; // Type of dogleg strategy to use. @@ -230,11 +346,11 @@ class Solver { // 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 + // fail. max_lm_diagonal and min_lm_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; + double min_lm_diagonal; + double max_lm_diagonal; // Sometimes due to numerical conditioning problems or linear // solver flakiness, the trust region strategy may return a @@ -269,11 +385,24 @@ class Solver { // Type of preconditioner to use with the iterative linear solvers. PreconditionerType preconditioner_type; + // Ceres supports using multiple dense linear algebra libraries + // for dense matrix factorizations. Currently EIGEN and LAPACK are + // the valid choices. EIGEN is always available, LAPACK refers to + // the system BLAS + LAPACK library which may or may not be + // available. + // + // This setting affects the DENSE_QR, DENSE_NORMAL_CHOLESKY and + // DENSE_SCHUR solvers. For small to moderate sized probem EIGEN + // is a fine choice but for large problems, an optimized LAPACK + + // BLAS implementation can make a substantial difference in + // performance. + DenseLinearAlgebraLibraryType dense_linear_algebra_library_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; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; // Number of threads used by Ceres to solve the Newton // step. Currently only the SPARSE_SCHUR solver is capable of @@ -351,9 +480,6 @@ class Solver { // deallocate the memory when destroyed. ParameterBlockOrdering* linear_solver_ordering; - // Note: This option only applies to the SPARSE_NORMAL_CHOLESKY - // solver when used with SUITE_SPARSE. - // Sparse Cholesky factorization algorithms use a fill-reducing // ordering to permute the columns of the Jacobian matrix. There // are two ways of doing this. @@ -372,7 +498,7 @@ class Solver { // In some rare cases, it is worth using a more complicated // reordering algorithm which has slightly better runtime // performance at the expense of an extra copy of the Jacobian - // // matrix. Setting use_postordering to true enables this tradeoff. + // matrix. Setting use_postordering to true enables this tradeoff. bool use_postordering; // Some non-linear least squares problems have additional @@ -443,18 +569,30 @@ class Solver { // // 2. Specify a collection of of ordered independent sets. Where // the lower numbered groups are optimized before the higher - // number groups. Each group must be an independent set. + // number groups. Each group must be an independent set. Not + // all parameter blocks need to be present in the ordering. ParameterBlockOrdering* inner_iteration_ordering; + // Generally speaking, inner iterations make significant progress + // in the early stages of the solve and then their contribution + // drops down sharply, at which point the time spent doing inner + // iterations is not worth it. + // + // Once the relative decrease in the objective function due to + // inner iterations drops below inner_iteration_tolerance, the use + // of inner iterations in subsequent trust region minimizer + // iterations is disabled. + double inner_iteration_tolerance; + // Minimum number of iterations for which the linear solver should // run, even if the convergence criterion is satisfied. - int linear_solver_min_num_iterations; + int min_linear_solver_iterations; // Maximum number of iterations for which the linear solver should // run. If the solver does not converge in less than - // linear_solver_max_num_iterations, then it returns - // MAX_ITERATIONS, as its termination type. - int linear_solver_max_num_iterations; + // max_linear_solver_iterations, then it returns MAX_ITERATIONS, + // as its termination type. + int max_linear_solver_iterations; // Forcing sequence parameter. The truncated Newton solver uses // this number to control the relative accuracy with which the @@ -480,14 +618,17 @@ class Solver { // is sent to STDOUT. bool minimizer_progress_to_stdout; - // List of iterations at which the optimizer should dump the - // linear least squares problem to disk. Useful for testing and - // benchmarking. If empty (default), no problems are dumped. - // - // This is ignored if protocol buffers are disabled. - vector<int> lsqp_iterations_to_dump; - string lsqp_dump_directory; - DumpFormatType lsqp_dump_format_type; + // List of iterations at which the minimizer should dump the trust + // region problem. Useful for testing and benchmarking. If empty + // (default), no problems are dumped. + vector<int> trust_region_minimizer_iterations_to_dump; + + // Directory to which the problems should be written to. Should be + // non-empty if trust_region_minimizer_iterations_to_dump is + // non-empty and trust_region_problem_dump_format_type is not + // CONSOLE. + string trust_region_problem_dump_directory; + DumpFormatType trust_region_problem_dump_format_type; // Finite differences options ---------------------------------------------- @@ -591,6 +732,9 @@ class Solver { int num_successful_steps; int num_unsuccessful_steps; + int num_inner_iteration_steps; + + // All times reported below are wall times. // When the user calls Solve, before the actual optimization // occurs, Ceres performs a number of preprocessing steps. These @@ -612,6 +756,7 @@ class Solver { double linear_solver_time_in_seconds; double residual_evaluation_time_in_seconds; double jacobian_evaluation_time_in_seconds; + double inner_iteration_time_in_seconds; // Preprocessor summary. int num_parameter_blocks; @@ -641,20 +786,26 @@ class Solver { vector<int> linear_solver_ordering_given; vector<int> linear_solver_ordering_used; + bool inner_iterations_given; + bool inner_iterations_used; + + vector<int> inner_iteration_ordering_given; + vector<int> inner_iteration_ordering_used; + PreconditionerType preconditioner_type; TrustRegionStrategyType trust_region_strategy_type; DoglegType dogleg_type; - bool inner_iterations; - SparseLinearAlgebraLibraryType sparse_linear_algebra_library; + DenseLinearAlgebraLibraryType dense_linear_algebra_library_type; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; LineSearchDirectionType line_search_direction_type; LineSearchType line_search_type; - int max_lbfgs_rank; + LineSearchInterpolationType line_search_interpolation_type; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; - vector<int> inner_iteration_ordering_given; - vector<int> inner_iteration_ordering_used; + int max_lbfgs_rank; }; // 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 5512340f7b3..ffa743a2d97 100644 --- a/extern/libmv/third_party/ceres/include/ceres/types.h +++ b/extern/libmv/third_party/ceres/include/ceres/types.h @@ -37,6 +37,8 @@ #ifndef CERES_PUBLIC_TYPES_H_ #define CERES_PUBLIC_TYPES_H_ +#include <string> + #include "ceres/internal/port.h" namespace ceres { @@ -124,6 +126,11 @@ enum SparseLinearAlgebraLibraryType { CX_SPARSE }; +enum DenseLinearAlgebraLibraryType { + EIGEN, + LAPACK +}; + enum LinearSolverTerminationType { // Termination criterion was met. For factorization based solvers // the tolerance is assumed to be zero. Any user provided values are @@ -167,10 +174,47 @@ enum LineSearchDirectionType { // used is determined by NonlinerConjuateGradientType. NONLINEAR_CONJUGATE_GRADIENT, - // A limited memory approximation to the inverse Hessian is - // maintained and used to compute a quasi-Newton step. + // BFGS, and it's limited memory approximation L-BFGS, are quasi-Newton + // algorithms that approximate the Hessian matrix by iteratively refining + // an initial estimate with rank-one updates using the gradient at each + // iteration. They are a generalisation of the Secant method and satisfy + // the Secant equation. The Secant equation has an infinium of solutions + // in multiple dimensions, as there are N*(N+1)/2 degrees of freedom in a + // symmetric matrix but only N conditions are specified by the Secant + // equation. The requirement that the Hessian approximation be positive + // definite imposes another N additional constraints, but that still leaves + // remaining degrees-of-freedom. (L)BFGS methods uniquely deteremine the + // approximate Hessian by imposing the additional constraints that the + // approximation at the next iteration must be the 'closest' to the current + // approximation (the nature of how this proximity is measured is actually + // the defining difference between a family of quasi-Newton methods including + // (L)BFGS & DFP). (L)BFGS is currently regarded as being the best known + // general quasi-Newton method. + // + // The principal difference between BFGS and L-BFGS is that whilst BFGS + // maintains a full, dense approximation to the (inverse) Hessian, L-BFGS + // maintains only a window of the last M observations of the parameters and + // gradients. Using this observation history, the calculation of the next + // search direction can be computed without requiring the construction of the + // full dense inverse Hessian approximation. This is particularly important + // for problems with a large number of parameters, where storage of an N-by-N + // matrix in memory would be prohibitive. + // + // For more details on BFGS see: + // + // Broyden, C.G., "The Convergence of a Class of Double-rank Minimization + // Algorithms,"; J. Inst. Maths. Applics., Vol. 6, pp 76–90, 1970. + // + // Fletcher, R., "A New Approach to Variable Metric Algorithms," + // Computer Journal, Vol. 13, pp 317–322, 1970. // - // For more details see + // Goldfarb, D., "A Family of Variable Metric Updates Derived by Variational + // Means," Mathematics of Computing, Vol. 24, pp 23–26, 1970. + // + // Shanno, D.F., "Conditioning of Quasi-Newton Methods for Function + // Minimization," Mathematics of Computing, Vol. 24, pp 647–656, 1970. + // + // For more details on L-BFGS see: // // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with Limited // Storage". Mathematics of Computation 35 (151): 773–782. @@ -179,7 +223,12 @@ enum LineSearchDirectionType { // "Representations of Quasi-Newton Matrices and their use in // Limited Memory Methods". Mathematical Programming 63 (4): // 129–156. + // + // A general reference for both methods: + // + // Nocedal J., Wright S., Numerical Optimization, 2nd Ed. Springer, 1999. LBFGS, + BFGS, }; // Nonliner conjugate gradient methods are a generalization of the @@ -198,6 +247,7 @@ enum LineSearchType { // Backtracking line search with polynomial interpolation or // bisection. ARMIJO, + WOLFE, }; // Ceres supports different strategies for computing the trust region @@ -310,13 +360,6 @@ enum DumpFormatType { CONSOLE, // Write out the linear least squares problem to the directory - // pointed to by Solver::Options::lsqp_dump_directory as a protocol - // buffer. linear_least_squares_problems.h/cc contains routines for - // loading these problems. For details on the on disk format used, - // see matrix.proto. The files are named lm_iteration_???.lsqp. - PROTOBUF, - - // Write out the linear least squares problem to the directory // pointed to by Solver::Options::lsqp_dump_directory as text files // which can be read into MATLAB/Octave. The Jacobian is dumped as a // text file containing (i,j,s) triplets, the vectors D, x and f are @@ -339,6 +382,18 @@ enum NumericDiffMethod { FORWARD }; +enum LineSearchInterpolationType { + BISECTION, + QUADRATIC, + CUBIC +}; + +enum CovarianceAlgorithmType { + DENSE_SVD, + SPARSE_CHOLESKY, + SPARSE_QR +}; + const char* LinearSolverTypeToString(LinearSolverType type); bool StringToLinearSolverType(string value, LinearSolverType* type); @@ -351,6 +406,12 @@ bool StringToSparseLinearAlgebraLibraryType( string value, SparseLinearAlgebraLibraryType* type); +const char* DenseLinearAlgebraLibraryTypeToString( + DenseLinearAlgebraLibraryType type); +bool StringToDenseLinearAlgebraLibraryType( + string value, + DenseLinearAlgebraLibraryType* type); + const char* TrustRegionStrategyTypeToString(TrustRegionStrategyType type); bool StringToTrustRegionStrategyType(string value, TrustRegionStrategyType* type); @@ -371,7 +432,20 @@ bool StringToLineSearchType(string value, LineSearchType* type); const char* NonlinearConjugateGradientTypeToString( NonlinearConjugateGradientType type); bool StringToNonlinearConjugateGradientType( - string value, NonlinearConjugateGradientType* type); + string value, + NonlinearConjugateGradientType* type); + +const char* LineSearchInterpolationTypeToString( + LineSearchInterpolationType type); +bool StringToLineSearchInterpolationType( + string value, + LineSearchInterpolationType* type); + +const char* CovarianceAlgorithmTypeToString( + CovarianceAlgorithmType type); +bool StringToCovarianceAlgorithmType( + string value, + CovarianceAlgorithmType* type); const char* LinearSolverTerminationTypeToString( LinearSolverTerminationType type); @@ -381,7 +455,8 @@ const char* SolverTerminationTypeToString(SolverTerminationType type); bool IsSchurType(LinearSolverType type); bool IsSparseLinearAlgebraLibraryTypeAvailable( SparseLinearAlgebraLibraryType type); - +bool IsDenseLinearAlgebraLibraryTypeAvailable( + DenseLinearAlgebraLibraryType type); } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/blas.cc b/extern/libmv/third_party/ceres/internal/ceres/blas.cc new file mode 100644 index 00000000000..f79b1ebfae1 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/blas.cc @@ -0,0 +1,78 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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/blas.h" +#include "glog/logging.h" + +extern "C" void dsyrk_(char* uplo, + char* trans, + int* n, + int* k, + double* alpha, + double* a, + int* lda, + double* beta, + double* c, + int* ldc); + +namespace ceres { +namespace internal { + +void BLAS::SymmetricRankKUpdate(int num_rows, + int num_cols, + const double* a, + bool transpose, + double alpha, + double beta, + double* c) { +#ifdef CERES_NO_LAPACK + LOG(FATAL) << "Ceres was built without a BLAS library."; +#else + char uplo = 'L'; + char trans = transpose ? 'T' : 'N'; + int n = transpose ? num_cols : num_rows; + int k = transpose ? num_rows : num_cols; + int lda = k; + int ldc = n; + dsyrk_(&uplo, + &trans, + &n, + &k, + &alpha, + const_cast<double*>(a), + &lda, + &beta, + c, + &ldc); +#endif +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/blas.h b/extern/libmv/third_party/ceres/internal/ceres/blas.h index 9629b3da550..2ab666395b9 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/blas.h +++ b/extern/libmv/third_party/ceres/internal/ceres/blas.h @@ -28,377 +28,28 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) // -// Simple blas functions for use in the Schur Eliminator. These are -// fairly basic implementations which already yield a significant -// speedup in the eliminator performance. +// Wrapper functions around BLAS functions. #ifndef CERES_INTERNAL_BLAS_H_ #define CERES_INTERNAL_BLAS_H_ -#include "ceres/internal/eigen.h" -#include "glog/logging.h" - namespace ceres { namespace internal { -// Remove the ".noalias()" annotation from the matrix matrix -// mutliplies 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() -= A * B; -// -// gets compiled to -// -// block.noalias() += A * B; -// -// 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. -#ifdef CERES_WORK_AROUND_ANDROID_NDK_COMPILER_BUG -#define CERES_MAYBE_NOALIAS -#else -#define CERES_MAYBE_NOALIAS .noalias() -#endif - -// The following three macros are used to share code and reduce -// template junk across the various GEMM variants. -#define CERES_GEMM_BEGIN(name) \ - template<int kRowA, int kColA, int kRowB, int kColB, int kOperation> \ - inline void name(const double* A, \ - const int num_row_a, \ - const int num_col_a, \ - const double* B, \ - const int num_row_b, \ - const int num_col_b, \ - double* C, \ - const int start_row_c, \ - const int start_col_c, \ - const int row_stride_c, \ - const int col_stride_c) - -#define CERES_GEMM_NAIVE_HEADER \ - DCHECK_GT(num_row_a, 0); \ - DCHECK_GT(num_col_a, 0); \ - DCHECK_GT(num_row_b, 0); \ - DCHECK_GT(num_col_b, 0); \ - DCHECK_GE(start_row_c, 0); \ - DCHECK_GE(start_col_c, 0); \ - DCHECK_GT(row_stride_c, 0); \ - DCHECK_GT(col_stride_c, 0); \ - DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); \ - DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); \ - DCHECK((kRowB == Eigen::Dynamic) || (kRowB == num_row_b)); \ - DCHECK((kColB == Eigen::Dynamic) || (kColB == num_col_b)); \ - const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); \ - const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); \ - const int NUM_ROW_B = (kColB != Eigen::Dynamic ? kRowB : num_row_b); \ - const int NUM_COL_B = (kColB != Eigen::Dynamic ? kColB : num_col_b); - -#define CERES_GEMM_EIGEN_HEADER \ - const typename EigenTypes<kRowA, kColA>::ConstMatrixRef \ - Aref(A, num_row_a, num_col_a); \ - const typename EigenTypes<kRowB, kColB>::ConstMatrixRef \ - Bref(B, num_row_b, num_col_b); \ - MatrixRef Cref(C, row_stride_c, col_stride_c); \ - -#define CERES_CALL_GEMM(name) \ - name<kRowA, kColA, kRowB, kColB, kOperation>( \ - A, num_row_a, num_col_a, \ - B, num_row_b, num_col_b, \ - C, start_row_c, start_col_c, row_stride_c, col_stride_c); - - -// For the matrix-matrix functions below, there are three variants for -// each functionality. Foo, FooNaive and FooEigen. Foo is the one to -// be called by the user. FooNaive is a basic loop based -// implementation and FooEigen uses Eigen's implementation. Foo -// chooses between FooNaive and FooEigen depending on how many of the -// template arguments are fixed at compile time. Currently, FooEigen -// is called if all matrix dimensions are compile time -// constants. FooNaive is called otherwise. This leads to the best -// performance currently. -// -// The MatrixMatrixMultiply variants compute: -// -// C op A * B; -// -// The MatrixTransposeMatrixMultiply variants compute: -// -// C op A' * B -// -// where op can be +=, -=, or =. -// -// The template parameters (kRowA, kColA, kRowB, kColB) allow -// specialization of the loop at compile time. If this information is -// not available, then Eigen::Dynamic should be used as the template -// argument. -// -// kOperation = 1 -> C += A * B -// kOperation = -1 -> C -= A * B -// kOperation = 0 -> C = A * B -// -// The functions can write into matrices C which are larger than the -// matrix A * B. This is done by specifying the true size of C via -// row_stride_c and col_stride_c, and then indicating where A * B -// should be written into by start_row_c and start_col_c. -// -// Graphically if row_stride_c = 10, col_stride_c = 12, start_row_c = -// 4 and start_col_c = 5, then if A = 3x2 and B = 2x4, we get -// -// ------------ -// ------------ -// ------------ -// ------------ -// -----xxxx--- -// -----xxxx--- -// -----xxxx--- -// ------------ -// ------------ -// ------------ -// -CERES_GEMM_BEGIN(MatrixMatrixMultiplyEigen) { - CERES_GEMM_EIGEN_HEADER - Eigen::Block<MatrixRef, kRowA, kColB> - block(Cref, start_row_c, start_col_c, num_row_a, num_col_b); - - if (kOperation > 0) { - block CERES_MAYBE_NOALIAS += Aref * Bref; - } else if (kOperation < 0) { - block CERES_MAYBE_NOALIAS -= Aref * Bref; - } else { - block CERES_MAYBE_NOALIAS = Aref * Bref; - } -} - -CERES_GEMM_BEGIN(MatrixMatrixMultiplyNaive) { - CERES_GEMM_NAIVE_HEADER - DCHECK_EQ(NUM_COL_A, NUM_ROW_B); - - const int NUM_ROW_C = NUM_ROW_A; - const int NUM_COL_C = NUM_COL_B; - DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c); - DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c); - - for (int row = 0; row < NUM_ROW_C; ++row) { - for (int col = 0; col < NUM_COL_C; ++col) { - double tmp = 0.0; - for (int k = 0; k < NUM_COL_A; ++k) { - tmp += A[row * NUM_COL_A + k] * B[k * NUM_COL_B + col]; - } - - const int index = (row + start_row_c) * col_stride_c + start_col_c + col; - if (kOperation > 0) { - C[index] += tmp; - } else if (kOperation < 0) { - C[index] -= tmp; - } else { - C[index] = tmp; - } - } - } -} - -CERES_GEMM_BEGIN(MatrixMatrixMultiply) { -#ifdef CERES_NO_CUSTOM_BLAS - - CERES_CALL_GEMM(MatrixMatrixMultiplyEigen) - return; - -#else - - if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic && - kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) { - CERES_CALL_GEMM(MatrixMatrixMultiplyEigen) - } else { - CERES_CALL_GEMM(MatrixMatrixMultiplyNaive) - } - -#endif -} - -CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyEigen) { - CERES_GEMM_EIGEN_HEADER - Eigen::Block<MatrixRef, kColA, kColB> block(Cref, - start_row_c, start_col_c, - num_col_a, num_col_b); - if (kOperation > 0) { - block CERES_MAYBE_NOALIAS += Aref.transpose() * Bref; - } else if (kOperation < 0) { - block CERES_MAYBE_NOALIAS -= Aref.transpose() * Bref; - } else { - block CERES_MAYBE_NOALIAS = Aref.transpose() * Bref; - } -} - -CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyNaive) { - CERES_GEMM_NAIVE_HEADER - DCHECK_EQ(NUM_ROW_A, NUM_ROW_B); - - const int NUM_ROW_C = NUM_COL_A; - const int NUM_COL_C = NUM_COL_B; - DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c); - DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c); - - for (int row = 0; row < NUM_ROW_C; ++row) { - for (int col = 0; col < NUM_COL_C; ++col) { - double tmp = 0.0; - for (int k = 0; k < NUM_ROW_A; ++k) { - tmp += A[k * NUM_COL_A + row] * B[k * NUM_COL_B + col]; - } - - const int index = (row + start_row_c) * col_stride_c + start_col_c + col; - if (kOperation > 0) { - C[index]+= tmp; - } else if (kOperation < 0) { - C[index]-= tmp; - } else { - C[index]= tmp; - } - } - } -} - -CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiply) { -#ifdef CERES_NO_CUSTOM_BLAS - - CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen) - return; - -#else - - if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic && - kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) { - CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen) - } else { - CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyNaive) - } - -#endif -} - -// Matrix-Vector multiplication -// -// c op A * b; -// -// where op can be +=, -=, or =. -// -// The template parameters (kRowA, kColA) allow specialization of the -// loop at compile time. If this information is not available, then -// Eigen::Dynamic should be used as the template argument. -// -// kOperation = 1 -> c += A' * b -// kOperation = -1 -> c -= A' * b -// kOperation = 0 -> c = A' * b -template<int kRowA, int kColA, int kOperation> -inline void MatrixVectorMultiply(const double* A, - const int num_row_a, - const int num_col_a, - const double* b, - double* c) { -#ifdef CERES_NO_CUSTOM_BLAS - const typename EigenTypes<kRowA, kColA>::ConstMatrixRef - Aref(A, num_row_a, num_col_a); - const typename EigenTypes<kColA>::ConstVectorRef bref(b, num_col_a); - typename EigenTypes<kRowA>::VectorRef cref(c, num_row_a); - - // lazyProduct works better than .noalias() for matrix-vector - // products. - if (kOperation > 0) { - cref += Aref.lazyProduct(bref); - } else if (kOperation < 0) { - cref -= Aref.lazyProduct(bref); - } else { - cref = Aref.lazyProduct(bref); - } -#else - - DCHECK_GT(num_row_a, 0); - DCHECK_GT(num_col_a, 0); - DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); - DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); - - const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); - const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); - - for (int row = 0; row < NUM_ROW_A; ++row) { - double tmp = 0.0; - for (int col = 0; col < NUM_COL_A; ++col) { - tmp += A[row * NUM_COL_A + col] * b[col]; - } - - if (kOperation > 0) { - c[row] += tmp; - } else if (kOperation < 0) { - c[row] -= tmp; - } else { - c[row] = tmp; - } - } -#endif // CERES_NO_CUSTOM_BLAS -} - -// Similar to MatrixVectorMultiply, except that A is transposed, i.e., -// -// c op A' * b; -template<int kRowA, int kColA, int kOperation> -inline void MatrixTransposeVectorMultiply(const double* A, - const int num_row_a, - const int num_col_a, - const double* b, - double* c) { -#ifdef CERES_NO_CUSTOM_BLAS - const typename EigenTypes<kRowA, kColA>::ConstMatrixRef - Aref(A, num_row_a, num_col_a); - const typename EigenTypes<kRowA>::ConstVectorRef bref(b, num_row_a); - typename EigenTypes<kColA>::VectorRef cref(c, num_col_a); - - // lazyProduct works better than .noalias() for matrix-vector - // products. - if (kOperation > 0) { - cref += Aref.transpose().lazyProduct(bref); - } else if (kOperation < 0) { - cref -= Aref.transpose().lazyProduct(bref); - } else { - cref = Aref.transpose().lazyProduct(bref); - } -#else - - DCHECK_GT(num_row_a, 0); - DCHECK_GT(num_col_a, 0); - DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); - DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); - - const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); - const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); - - for (int row = 0; row < NUM_COL_A; ++row) { - double tmp = 0.0; - for (int col = 0; col < NUM_ROW_A; ++col) { - tmp += A[col * NUM_COL_A + row] * b[col]; - } - - if (kOperation > 0) { - c[row] += tmp; - } else if (kOperation < 0) { - c[row] -= tmp; - } else { - c[row] = tmp; - } - } -#endif // CERES_NO_CUSTOM_BLAS -} - - -#undef CERES_MAYBE_NOALIAS -#undef CERES_GEMM_BEGIN -#undef CERES_GEMM_EIGEN_HEADER -#undef CERES_GEMM_NAIVE_HEADER -#undef CERES_CALL_GEMM +class BLAS { + public: + // transpose = true : c = alpha * a'a + beta * c; + // transpose = false : c = alpha * aa' + beta * c; + // + // Assumes column major matrices. + static void SymmetricRankKUpdate(int num_rows, + int num_cols, + const double* a, + bool transpose, + double alpha, + double beta, + double* c); +}; } // namespace internal } // namespace ceres 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 1d5f9d77ab0..29974d45bc9 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 @@ -41,7 +41,7 @@ namespace ceres { namespace internal { BlockJacobiPreconditioner::BlockJacobiPreconditioner( - const BlockSparseMatrixBase& A) + const BlockSparseMatrix& A) : num_rows_(A.num_rows()), block_structure_(*A.block_structure()) { // Calculate the amount of storage needed. @@ -66,19 +66,19 @@ BlockJacobiPreconditioner::BlockJacobiPreconditioner( BlockJacobiPreconditioner::~BlockJacobiPreconditioner() {} -bool BlockJacobiPreconditioner::Update(const BlockSparseMatrixBase& A, - const double* D) { +bool BlockJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, + const double* D) { const CompressedRowBlockStructure* bs = A.block_structure(); // Compute the diagonal blocks by block inner products. std::fill(block_storage_.begin(), block_storage_.end(), 0.0); + const double* values = A.values(); for (int r = 0; r < bs->rows.size(); ++r) { const int row_block_size = bs->rows[r].block.size; const vector<Cell>& cells = bs->rows[r].cells; - const double* row_values = A.RowBlockValues(r); for (int c = 0; c < cells.size(); ++c) { const int col_block_size = bs->cols[cells[c].block_id].size; - ConstMatrixRef m(row_values + cells[c].position, + ConstMatrixRef m(values + cells[c].position, row_block_size, col_block_size); @@ -111,7 +111,7 @@ bool BlockJacobiPreconditioner::Update(const BlockSparseMatrixBase& A, } block = block.selfadjointView<Eigen::Upper>() - .ldlt() + .llt() .solve(Matrix::Identity(size, size)); } return true; diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h index ed5eebc8dc6..3505a01248b 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h @@ -37,7 +37,7 @@ namespace ceres { namespace internal { -class BlockSparseMatrixBase; +class BlockSparseMatrix; struct CompressedRowBlockStructure; class LinearOperator; @@ -51,20 +51,21 @@ class LinearOperator; // update the matrix by running Update(A, D). The values of the matrix A are // inspected to construct the preconditioner. The vector D is applied as the // D^TD diagonal term. -class BlockJacobiPreconditioner : public Preconditioner { +class BlockJacobiPreconditioner : public BlockSparseMatrixPreconditioner { public: // A must remain valid while the BlockJacobiPreconditioner is. - explicit BlockJacobiPreconditioner(const BlockSparseMatrixBase& A); + explicit BlockJacobiPreconditioner(const BlockSparseMatrix& A); virtual ~BlockJacobiPreconditioner(); // Preconditioner interface - virtual bool Update(const BlockSparseMatrixBase& A, const double* D); virtual void RightMultiply(const double* x, double* y) const; virtual void LeftMultiply(const double* x, double* y) const; virtual int num_rows() const { return num_rows_; } virtual int num_cols() const { return num_rows_; } private: + virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); + std::vector<double*> blocks_; std::vector<double> block_storage_; int num_rows_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.cc new file mode 100644 index 00000000000..5b008e2c3d8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.cc @@ -0,0 +1,170 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_crs_matrix.h" + +#include <algorithm> +#include <set> +#include <utility> +#include <vector> +#include "ceres/compressed_row_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 { + +BlockRandomAccessCRSMatrix::BlockRandomAccessCRSMatrix( + const vector<int>& blocks, + const set<pair<int, int> >& block_pairs) + : kMaxRowBlocks(10 * 1000 * 1000), + blocks_(blocks) { + CHECK_LT(blocks.size(), kMaxRowBlocks); + + col_layout_.resize(blocks_.size(), 0); + row_strides_.resize(blocks_.size(), 0); + + // Build the row/column layout vector and count the number of scalar + // rows/columns. + int num_cols = 0; + for (int i = 0; i < blocks_.size(); ++i) { + col_layout_[i] = num_cols; + num_cols += blocks_[i]; + } + + // Walk the sparsity pattern and count the number of non-zeros. + int num_nonzeros = 0; + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_size = blocks_[it->first]; + const int col_block_size = blocks_[it->second]; + num_nonzeros += row_block_size * col_block_size; + } + + VLOG(2) << "Matrix Size [" << num_cols + << "," << num_cols + << "] " << num_nonzeros; + + crsm_.reset(new CompressedRowSparseMatrix(num_cols, num_cols, num_nonzeros)); + int* rows = crsm_->mutable_rows(); + int* cols = crsm_->mutable_cols(); + double* values = crsm_->mutable_values(); + + // Iterate over the sparsity pattern and fill the scalar sparsity + // pattern of the underlying compressed sparse row matrix. Along the + // way also fill out the Layout object which will allow random + // access into the CRS Matrix. + set<pair<int, int> >::const_iterator it = block_pairs.begin(); + vector<int> col_blocks; + int row_pos = 0; + rows[0] = 0; + while (it != block_pairs.end()) { + // Add entries to layout_ for all the blocks for this row. + col_blocks.clear(); + const int row_block_id = it->first; + const int row_block_size = blocks_[row_block_id]; + int num_cols = 0; + while ((it != block_pairs.end()) && (it->first == row_block_id)) { + layout_[IntPairToLong(it->first, it->second)] = + new CellInfo(values + num_cols); + col_blocks.push_back(it->second); + num_cols += blocks_[it->second]; + ++it; + }; + + // Count the number of non-zeros in the row block. + for (int j = 0; j < row_block_size; ++j) { + rows[row_pos + j + 1] = rows[row_pos + j] + num_cols; + } + + // Fill out the sparsity pattern for each row. + int col_pos = 0; + for (int j = 0; j < col_blocks.size(); ++j) { + const int col_block_id = col_blocks[j]; + const int col_block_size = blocks_[col_block_id]; + for (int r = 0; r < row_block_size; ++r) { + const int column_block_begin = rows[row_pos + r] + col_pos; + for (int c = 0; c < col_block_size; ++c) { + cols[column_block_begin + c] = col_layout_[col_block_id] + c; + } + } + col_pos += col_block_size; + } + + row_pos += row_block_size; + values += row_block_size * num_cols; + row_strides_[row_block_id] = num_cols; + } +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +BlockRandomAccessCRSMatrix::~BlockRandomAccessCRSMatrix() { + // TODO(sameeragarwal) this should be rationalized going forward and + // perhaps moved into BlockRandomAccessMatrix. + for (LayoutType::iterator it = layout_.begin(); + it != layout_.end(); + ++it) { + delete it->second; + } +} + +CellInfo* BlockRandomAccessCRSMatrix::GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) { + const LayoutType::iterator it = + layout_.find(IntPairToLong(row_block_id, col_block_id)); + if (it == layout_.end()) { + return NULL; + } + + *row = 0; + *col = 0; + *row_stride = blocks_[row_block_id]; + *col_stride = row_strides_[row_block_id]; + return it->second; +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +void BlockRandomAccessCRSMatrix::SetZero() { + crsm_->SetZero(); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.h new file mode 100644 index 00000000000..11a203b8f4d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.h @@ -0,0 +1,108 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_CRS_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_CRS_MATRIX_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/mutex.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/collections_port.h" +#include "ceres/integral_types.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// A square BlockRandomAccessMatrix where the underlying storage is a +// compressed row sparse matrix. The matrix need not be symmetric. +class BlockRandomAccessCRSMatrix : public BlockRandomAccessMatrix { + public: + // blocks is an array of block sizes. block_pairs is a set of + // <row_block_id, col_block_id> pairs to identify the non-zero cells + // of this matrix. + BlockRandomAccessCRSMatrix(const vector<int>& blocks, + const set<pair<int, int> >& block_pairs); + + // The destructor is not thread safe. It assumes that no one is + // modifying any cells when the matrix is being destroyed. + virtual ~BlockRandomAccessCRSMatrix(); + + // BlockRandomAccessMatrix Interface. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride); + + // This is not a thread safe method, it assumes that no cell is + // locked. + virtual void SetZero(); + + // Since the matrix is square, num_rows() == num_cols(). + virtual int num_rows() const { return crsm_->num_rows(); } + virtual int num_cols() const { return crsm_->num_cols(); } + + // Access to the underlying matrix object. + const CompressedRowSparseMatrix* matrix() const { return crsm_.get(); } + CompressedRowSparseMatrix* mutable_matrix() { return crsm_.get(); } + + private: + int64 IntPairToLong(int a, int b) { + return a * kMaxRowBlocks + b; + } + + const int64 kMaxRowBlocks; + // row/column block sizes. + const vector<int> blocks_; + vector<int> col_layout_; + vector<int> row_strides_; + + // A mapping from <row_block_id, col_block_id> to the position in + // the values array of tsm_ where the block is stored. + typedef HashMap<long int, CellInfo* > LayoutType; + LayoutType layout_; + + scoped_ptr<CompressedRowSparseMatrix> crsm_; + friend class BlockRandomAccessCRSMatrixTest; + CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessCRSMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_CRS_MATRIX_H_ 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 48a00437cf6..a6b5f39a985 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 @@ -74,7 +74,6 @@ class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { // This is not a thread safe method, it assumes that no cell is // locked. virtual void SetZero(); - virtual bool IsThreadSafe() const { return true; } // Since the matrix is square, num_rows() == num_cols(). virtual int num_rows() const { return tsm_->num_rows(); } 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 ae36d60c900..a4872626114 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,10 +33,9 @@ #include <cstddef> #include <algorithm> #include <vector> -#include "ceres/blas.h" #include "ceres/block_structure.h" #include "ceres/internal/eigen.h" -#include "ceres/matrix_proto.h" +#include "ceres/small_blas.h" #include "ceres/triplet_sparse_matrix.h" #include "glog/logging.h" @@ -82,31 +81,6 @@ BlockSparseMatrix::BlockSparseMatrix( CHECK_NOTNULL(values_.get()); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -BlockSparseMatrix::BlockSparseMatrix(const SparseMatrixProto& outer_proto) { - CHECK(outer_proto.has_block_matrix()); - - const BlockSparseMatrixProto& proto = outer_proto.block_matrix(); - CHECK(proto.has_num_rows()); - CHECK(proto.has_num_cols()); - CHECK_EQ(proto.num_nonzeros(), proto.values_size()); - - num_rows_ = proto.num_rows(); - num_cols_ = proto.num_cols(); - num_nonzeros_ = proto.num_nonzeros(); - - // Copy out the values into *this. - values_.reset(new double[num_nonzeros_]); - for (int i = 0; i < proto.num_nonzeros(); ++i) { - values_[i] = proto.values(i); - } - - // Create the block structure according to the proto. - block_structure_.reset(new CompressedRowBlockStructure); - ProtoToBlockStructure(proto.block_structure(), block_structure_.get()); -} -#endif - void BlockSparseMatrix::SetZero() { fill(values_.get(), values_.get() + num_nonzeros_, 0.0); } @@ -243,21 +217,6 @@ const CompressedRowBlockStructure* BlockSparseMatrix::block_structure() return block_structure_.get(); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -void BlockSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { - outer_proto->Clear(); - - BlockSparseMatrixProto* proto = outer_proto->mutable_block_matrix(); - proto->set_num_rows(num_rows_); - proto->set_num_cols(num_cols_); - proto->set_num_nonzeros(num_nonzeros_); - for (int i = 0; i < num_nonzeros_; ++i) { - proto->add_values(values_[i]); - } - BlockStructureToProto(*block_structure_, proto->mutable_block_structure()); -} -#endif - void BlockSparseMatrix::ToTextFile(FILE* file) const { CHECK_NOTNULL(file); for (int i = 0; i < block_structure_->rows.size(); ++i) { 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 513d398c54d..e17d12a706e 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 @@ -43,40 +43,8 @@ namespace ceres { namespace internal { -class SparseMatrixProto; class TripletSparseMatrix; -// A further extension of the SparseMatrix interface to support block-oriented -// matrices. The key addition is the RowBlockValues() accessor, which enables -// the lazy block sparse matrix implementation. -class BlockSparseMatrixBase : public SparseMatrix { - public: - BlockSparseMatrixBase() {} - virtual ~BlockSparseMatrixBase() {} - - // Convert this matrix into a triplet sparse matrix. - virtual void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const = 0; - - // Returns a pointer to the block structure. Does not transfer - // ownership. - virtual const CompressedRowBlockStructure* block_structure() const = 0; - - // Returns a pointer to a row of the matrix. The returned array is only valid - // until the next call to RowBlockValues. The caller does not own the result. - // - // The returned array is laid out such that cells on the specified row are - // contiguous in the returned array, though neighbouring cells in row order - // may not be contiguous in the row values. The cell values for cell - // (row_block, cell_block) are found at offset - // - // block_structure()->rows[row_block].cells[cell_block].position - // - virtual const double* RowBlockValues(int row_block_index) const = 0; - - private: - CERES_DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrixBase); -}; - // This class implements the SparseMatrix interface for storing and // manipulating block sparse matrices. The block structure is stored // in the CompressedRowBlockStructure object and one is needed to @@ -85,7 +53,7 @@ class BlockSparseMatrixBase : public SparseMatrix { // // internal/ceres/block_structure.h // -class BlockSparseMatrix : public BlockSparseMatrixBase { +class BlockSparseMatrix : public SparseMatrix { public: // Construct a block sparse matrix with a fully initialized // CompressedRowBlockStructure objected. The matrix takes over @@ -95,11 +63,6 @@ class BlockSparseMatrix : public BlockSparseMatrixBase { // CompressedRowBlockStructure objects. explicit BlockSparseMatrix(CompressedRowBlockStructure* block_structure); - // Construct a block sparse matrix from a protocol buffer. -#ifndef CERES_NO_PROTOCOL_BUFFERS - explicit BlockSparseMatrix(const SparseMatrixProto& proto); -#endif - BlockSparseMatrix(); virtual ~BlockSparseMatrix(); @@ -110,9 +73,6 @@ 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_NO_PROTOCOL_BUFFERS - virtual void ToProto(SparseMatrixProto* proto) const; -#endif virtual void ToTextFile(FILE* file) const; virtual int num_rows() const { return num_rows_; } @@ -121,12 +81,8 @@ class BlockSparseMatrix : public BlockSparseMatrixBase { virtual const double* values() const { return values_.get(); } virtual double* mutable_values() { return values_.get(); } - // Implementation of BlockSparseMatrixBase interface. - virtual void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const; - virtual const CompressedRowBlockStructure* block_structure() const; - virtual const double* RowBlockValues(int row_block_index) const { - return values_.get(); - } + void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const; + const CompressedRowBlockStructure* block_structure() const; private: int num_rows_; 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 e61131192af..5a1a5e18336 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc @@ -29,7 +29,6 @@ // Author: sameeragarwal@google.com (Sameer Agarwal) #include "ceres/block_structure.h" -#include "ceres/matrix_proto.h" namespace ceres { namespace internal { @@ -38,55 +37,5 @@ bool CellLessThan(const Cell& lhs, const Cell& rhs) { return (lhs.block_id < rhs.block_id); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -void ProtoToBlockStructure(const BlockStructureProto &proto, - CompressedRowBlockStructure *block_structure) { - // Decode the column blocks. - block_structure->cols.resize(proto.cols_size()); - for (int i = 0; i < proto.cols_size(); ++i) { - block_structure->cols[i].size = proto.cols(i).size(); - block_structure->cols[i].position = - proto.cols(i).position(); - } - // Decode the row structure. - block_structure->rows.resize(proto.rows_size()); - for (int i = 0; i < proto.rows_size(); ++i) { - const CompressedRowProto &row = proto.rows(i); - block_structure->rows[i].block.size = row.block().size(); - block_structure->rows[i].block.position = row.block().position(); - - // Copy the cells within the row. - block_structure->rows[i].cells.resize(row.cells_size()); - for (int j = 0; j < row.cells_size(); ++j) { - const CellProto &cell = row.cells(j); - block_structure->rows[i].cells[j].block_id = cell.block_id(); - block_structure->rows[i].cells[j].position = cell.position(); - } - } -} - -void BlockStructureToProto(const CompressedRowBlockStructure &block_structure, - BlockStructureProto *proto) { - // Encode the column blocks. - for (int i = 0; i < block_structure.cols.size(); ++i) { - BlockProto *block = proto->add_cols(); - block->set_size(block_structure.cols[i].size); - block->set_position(block_structure.cols[i].position); - } - // Encode the row structure. - for (int i = 0; i < block_structure.rows.size(); ++i) { - CompressedRowProto *row = proto->add_rows(); - BlockProto *block = row->mutable_block(); - block->set_size(block_structure.rows[i].block.size); - block->set_position(block_structure.rows[i].block.position); - for (int j = 0; j < block_structure.rows[i].cells.size(); ++j) { - CellProto *cell = row->add_cells(); - cell->set_block_id(block_structure.rows[i].cells[j].block_id); - cell->set_position(block_structure.rows[i].cells[j].position); - } - } -} -#endif - } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/c_api.cc b/extern/libmv/third_party/ceres/internal/ceres/c_api.cc new file mode 100644 index 00000000000..1fd01c9f0bd --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/c_api.cc @@ -0,0 +1,188 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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: mierle@gmail.com (Keir Mierle) +// +// An incomplete C API for Ceres. +// +// TODO(keir): Figure out why logging does not seem to work. + +#include "ceres/c_api.h" + +#include <vector> +#include <iostream> +#include <string> +#include "ceres/cost_function.h" +#include "ceres/loss_function.h" +#include "ceres/problem.h" +#include "ceres/solver.h" +#include "ceres/types.h" // for std +#include "glog/logging.h" + +using ceres::Problem; + +void ceres_init() { + // This is not ideal, but it's not clear what to do if there is no gflags and + // no access to command line arguments. + char message[] = "<unknown>"; + google::InitGoogleLogging(message); +} + +ceres_problem_t* ceres_create_problem() { + return reinterpret_cast<ceres_problem_t*>(new Problem); +} + +void ceres_free_problem(ceres_problem_t* problem) { + delete reinterpret_cast<Problem*>(problem); +} + +// This cost function wraps a C-level function pointer from the user, to bridge +// between C and C++. +class CallbackCostFunction : public ceres::CostFunction { + public: + CallbackCostFunction(ceres_cost_function_t cost_function, + void* user_data, + int num_residuals, + int num_parameter_blocks, + int* parameter_block_sizes) + : cost_function_(cost_function), + user_data_(user_data) { + set_num_residuals(num_residuals); + for (int i = 0; i < num_parameter_blocks; ++i) { + mutable_parameter_block_sizes()->push_back(parameter_block_sizes[i]); + } + } + + virtual ~CallbackCostFunction() {} + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + return (*cost_function_)(user_data_, + const_cast<double**>(parameters), + residuals, + jacobians); + } + + private: + ceres_cost_function_t cost_function_; + void* user_data_; +}; + +// This loss function wraps a C-level function pointer from the user, to bridge +// between C and C++. +class CallbackLossFunction : public ceres::LossFunction { + public: + explicit CallbackLossFunction(ceres_loss_function_t loss_function, + void* user_data) + : loss_function_(loss_function), user_data_(user_data) {} + virtual void Evaluate(double sq_norm, double* rho) const { + (*loss_function_)(user_data_, sq_norm, rho); + } + + private: + ceres_loss_function_t loss_function_; + void* user_data_; +}; + +// Wrappers for the stock loss functions. +void* ceres_create_huber_loss_function_data(double a) { + return new ceres::HuberLoss(a); +} +void* ceres_create_softl1_loss_function_data(double a) { + return new ceres::SoftLOneLoss(a); +} +void* ceres_create_cauchy_loss_function_data(double a) { + return new ceres::CauchyLoss(a); +} +void* ceres_create_arctan_loss_function_data(double a) { + return new ceres::ArctanLoss(a); +} +void* ceres_create_tolerant_loss_function_data(double a, double b) { + return new ceres::TolerantLoss(a, b); +} + +void ceres_free_stock_loss_function_data(void* loss_function_data) { + delete reinterpret_cast<ceres::LossFunction*>(loss_function_data); +} + +void ceres_stock_loss_function(void* user_data, + double squared_norm, + double out[3]) { + reinterpret_cast<ceres::LossFunction*>(user_data) + ->Evaluate(squared_norm, out); +} + +ceres_residual_block_id_t* ceres_problem_add_residual_block( + ceres_problem_t* problem, + ceres_cost_function_t cost_function, + void* cost_function_data, + ceres_loss_function_t loss_function, + void* loss_function_data, + int num_residuals, + int num_parameter_blocks, + int* parameter_block_sizes, + double** parameters) { + Problem* ceres_problem = reinterpret_cast<Problem*>(problem); + + ceres::CostFunction* callback_cost_function = + new CallbackCostFunction(cost_function, + cost_function_data, + num_residuals, + num_parameter_blocks, + parameter_block_sizes); + + ceres::LossFunction* callback_loss_function = NULL; + if (loss_function != NULL) { + callback_loss_function = new CallbackLossFunction(loss_function, + loss_function_data); + } + + std::vector<double*> parameter_blocks(parameters, + parameters + num_parameter_blocks); + return reinterpret_cast<ceres_residual_block_id_t*>( + ceres_problem->AddResidualBlock(callback_cost_function, + callback_loss_function, + parameter_blocks)); +} + +void ceres_solve(ceres_problem_t* c_problem) { + Problem* problem = reinterpret_cast<Problem*>(c_problem); + + // TODO(keir): Obviously, this way of setting options won't scale or last. + // Instead, figure out a way to specify some of the options without + // duplicating everything. + ceres::Solver::Options options; + options.max_num_iterations = 100; + options.linear_solver_type = ceres::DENSE_QR; + options.minimizer_progress_to_stdout = true; + + ceres::Solver::Summary summary; + ceres::Solve(options, problem, &summary); + std::cout << summary.FullReport() << "\n"; +} diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc index e2e799fe607..9b8f9808cc9 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc @@ -46,7 +46,7 @@ CgnrSolver::CgnrSolver(const LinearSolver::Options& options) } LinearSolver::Summary CgnrSolver::SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { 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 d560a9de58d..c63484c628b 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h @@ -48,11 +48,11 @@ class BlockJacobiPreconditioner; // // as required for solving for x in the least squares sense. Currently only // block diagonal preconditioning is supported. -class CgnrSolver : public BlockSparseMatrixBaseSolver { +class CgnrSolver : public BlockSparseMatrixSolver { public: explicit CgnrSolver(const LinearSolver::Options& options); virtual Summary SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x); diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc b/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc new file mode 100644 index 00000000000..b62a6ed3830 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc @@ -0,0 +1,118 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/compressed_col_sparse_matrix_utils.h" + +#include <vector> +#include <algorithm> +#include "ceres/internal/port.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +void CompressedColumnScalarMatrixToBlockMatrix(const int* scalar_rows, + const int* scalar_cols, + 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]; + } + + // This loop extracts the block sparsity of the scalar sparse matrix + // 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 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++; + } + } +} +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h b/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h new file mode 100644 index 00000000000..c8de2a1591a --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h @@ -0,0 +1,142 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ +#define CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ + +#include <vector> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +// Extract the block sparsity pattern of the scalar compressed columns +// matrix 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. +void CompressedColumnScalarMatrixToBlockMatrix(const int* scalar_rows, + const int* scalar_cols, + const vector<int>& row_blocks, + const vector<int>& col_blocks, + vector<int>* block_rows, + vector<int>* block_cols); + +// Given a set of blocks and a permutation of these blocks, compute +// the corresponding "scalar" ordering, where the scalar ordering of +// size sum(blocks). +void BlockOrderingToScalarOrdering(const vector<int>& blocks, + const vector<int>& block_ordering, + vector<int>* scalar_ordering); + +// Solve the linear system +// +// R * solution = rhs +// +// Where R is an upper triangular compressed column sparse matrix. +template <typename IntegerType> +void SolveUpperTriangularInPlace(IntegerType num_cols, + const IntegerType* rows, + const IntegerType* cols, + const double* values, + double* rhs_and_solution) { + for (IntegerType c = num_cols - 1; c >= 0; --c) { + rhs_and_solution[c] /= values[cols[c + 1] - 1]; + for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { + const IntegerType r = rows[idx]; + const double v = values[idx]; + rhs_and_solution[r] -= v * rhs_and_solution[c]; + } + } +} + +// Solve the linear system +// +// R' * solution = rhs +// +// Where R is an upper triangular compressed column sparse matrix. +template <typename IntegerType> +void SolveUpperTriangularTransposeInPlace(IntegerType num_cols, + const IntegerType* rows, + const IntegerType* cols, + const double* values, + double* rhs_and_solution) { + for (IntegerType c = 0; c < num_cols; ++c) { + for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { + const IntegerType r = rows[idx]; + const double v = values[idx]; + rhs_and_solution[c] -= v * rhs_and_solution[r]; + } + rhs_and_solution[c] = rhs_and_solution[c] / values[cols[c + 1] - 1]; + } +} + +// Given a upper triangular matrix R in compressed column form, solve +// the linear system, +// +// R'R x = b +// +// Where b is all zeros except for rhs_nonzero_index, where it is +// equal to one. +// +// The function exploits this knowledge to reduce the number of +// floating point operations. +template <typename IntegerType> +void SolveRTRWithSparseRHS(IntegerType num_cols, + const IntegerType* rows, + const IntegerType* cols, + const double* values, + const int rhs_nonzero_index, + double* solution) { + fill(solution, solution + num_cols, 0.0); + solution[rhs_nonzero_index] = 1.0 / values[cols[rhs_nonzero_index + 1] - 1]; + + for (IntegerType c = rhs_nonzero_index + 1; c < num_cols; ++c) { + for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { + const IntegerType r = rows[idx]; + if (r < rhs_nonzero_index) continue; + const double v = values[idx]; + solution[c] -= v * solution[r]; + } + solution[c] = solution[c] / values[cols[c + 1] - 1]; + } + + SolveUpperTriangularInPlace(num_cols, rows, cols, values, solution); +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc index 1b61468aaae..e200c928509 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 @@ -34,7 +34,8 @@ #include <vector> #include "ceres/crs_matrix.h" #include "ceres/internal/port.h" -#include "ceres/matrix_proto.h" +#include "ceres/triplet_sparse_matrix.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -72,28 +73,27 @@ CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows, int max_num_nonzeros) { num_rows_ = num_rows; num_cols_ = num_cols; - max_num_nonzeros_ = max_num_nonzeros; + rows_.resize(num_rows + 1, 0); + cols_.resize(max_num_nonzeros, 0); + values_.resize(max_num_nonzeros, 0.0); - VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ - << " max_num_nonzeros: " << max_num_nonzeros_ - << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT - max_num_nonzeros_ * sizeof(int) + // NOLINT - max_num_nonzeros_ * sizeof(double); // NOLINT - - rows_.reset(new int[num_rows_ + 1]); - cols_.reset(new int[max_num_nonzeros_]); - values_.reset(new double[max_num_nonzeros_]); - fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); - fill(cols_.get(), cols_.get() + max_num_nonzeros_, 0); - fill(values_.get(), values_.get() + max_num_nonzeros_, 0); + VLOG(1) << "# of rows: " << num_rows_ + << " # of columns: " << num_cols_ + << " max_num_nonzeros: " << cols_.size() + << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT + cols_.size() * sizeof(int) + // NOLINT + cols_.size() * sizeof(double); // NOLINT } CompressedRowSparseMatrix::CompressedRowSparseMatrix( const TripletSparseMatrix& m) { num_rows_ = m.num_rows(); num_cols_ = m.num_cols(); - max_num_nonzeros_ = m.max_num_nonzeros(); + + rows_.resize(num_rows_ + 1, 0); + cols_.resize(m.num_nonzeros(), 0); + values_.resize(m.max_num_nonzeros(), 0.0); // index is the list of indices into the TripletSparseMatrix m. vector<int> index(m.num_nonzeros(), 0); @@ -105,18 +105,13 @@ CompressedRowSparseMatrix::CompressedRowSparseMatrix( // are broken by column. sort(index.begin(), index.end(), RowColLessThan(m.rows(), m.cols())); - VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ - << " max_num_nonzeros: " << max_num_nonzeros_ - << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT - max_num_nonzeros_ * sizeof(int) + // NOLINT - max_num_nonzeros_ * sizeof(double); // NOLINT - - rows_.reset(new int[num_rows_ + 1]); - cols_.reset(new int[max_num_nonzeros_]); - values_.reset(new double[max_num_nonzeros_]); - - // rows_ = 0 - fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); + VLOG(1) << "# of rows: " << num_rows_ + << " # of columns: " << num_cols_ + << " max_num_nonzeros: " << cols_.size() + << ". Allocating " + << ((num_rows_ + 1) * sizeof(int) + // NOLINT + cols_.size() * sizeof(int) + // NOLINT + cols_.size() * sizeof(double)); // NOLINT // Copy the contents of the cols and values array in the order given // by index and count the number of entries in each row. @@ -135,49 +130,15 @@ CompressedRowSparseMatrix::CompressedRowSparseMatrix( CHECK_EQ(num_nonzeros(), m.num_nonzeros()); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -CompressedRowSparseMatrix::CompressedRowSparseMatrix( - const SparseMatrixProto& outer_proto) { - CHECK(outer_proto.has_compressed_row_matrix()); - - const CompressedRowSparseMatrixProto& proto = - outer_proto.compressed_row_matrix(); - - num_rows_ = proto.num_rows(); - num_cols_ = proto.num_cols(); - - rows_.reset(new int[proto.rows_size()]); - cols_.reset(new int[proto.cols_size()]); - values_.reset(new double[proto.values_size()]); - - for (int i = 0; i < proto.rows_size(); ++i) { - rows_[i] = proto.rows(i); - } - - CHECK_EQ(proto.rows_size(), num_rows_ + 1); - CHECK_EQ(proto.cols_size(), proto.values_size()); - CHECK_EQ(proto.cols_size(), rows_[num_rows_]); - - for (int i = 0; i < proto.cols_size(); ++i) { - cols_[i] = proto.cols(i); - values_[i] = proto.values(i); - } - - max_num_nonzeros_ = proto.cols_size(); -} -#endif - CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, int num_rows) { CHECK_NOTNULL(diagonal); num_rows_ = num_rows; num_cols_ = num_rows; - max_num_nonzeros_ = num_rows; - - rows_.reset(new int[num_rows_ + 1]); - cols_.reset(new int[num_rows_]); - values_.reset(new double[num_rows_]); + rows_.resize(num_rows + 1); + cols_.resize(num_rows); + values_.resize(num_rows); rows_[0] = 0; for (int i = 0; i < num_rows_; ++i) { @@ -193,7 +154,7 @@ CompressedRowSparseMatrix::~CompressedRowSparseMatrix() { } void CompressedRowSparseMatrix::SetZero() { - fill(values_.get(), values_.get() + num_nonzeros(), 0.0); + fill(values_.begin(), values_.end(), 0); } void CompressedRowSparseMatrix::RightMultiply(const double* x, @@ -248,83 +209,35 @@ void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { } } -#ifndef CERES_NO_PROTOCOL_BUFFERS -void CompressedRowSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { - CHECK_NOTNULL(outer_proto); - - outer_proto->Clear(); - CompressedRowSparseMatrixProto* proto - = outer_proto->mutable_compressed_row_matrix(); - - proto->set_num_rows(num_rows_); - proto->set_num_cols(num_cols_); - - for (int r = 0; r < num_rows_ + 1; ++r) { - proto->add_rows(rows_[r]); - } - - for (int idx = 0; idx < rows_[num_rows_]; ++idx) { - proto->add_cols(cols_[idx]); - proto->add_values(values_[idx]); - } -} -#endif - void CompressedRowSparseMatrix::DeleteRows(int delta_rows) { CHECK_GE(delta_rows, 0); CHECK_LE(delta_rows, num_rows_); - int new_num_rows = num_rows_ - delta_rows; - - num_rows_ = new_num_rows; - int* new_rows = new int[num_rows_ + 1]; - copy(rows_.get(), rows_.get() + num_rows_ + 1, new_rows); - rows_.reset(new_rows); + num_rows_ -= delta_rows; + rows_.resize(num_rows_ + 1); } void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { CHECK_EQ(m.num_cols(), num_cols_); - // Check if there is enough space. If not, then allocate new arrays - // to hold the combined matrix and copy the contents of this matrix - // into it. - if (max_num_nonzeros_ < num_nonzeros() + m.num_nonzeros()) { - int new_max_num_nonzeros = num_nonzeros() + m.num_nonzeros(); - - VLOG(1) << "Reallocating " << sizeof(int) * new_max_num_nonzeros; // NOLINT - - int* new_cols = new int[new_max_num_nonzeros]; - copy(cols_.get(), cols_.get() + max_num_nonzeros_, new_cols); - cols_.reset(new_cols); - - double* new_values = new double[new_max_num_nonzeros]; - copy(values_.get(), values_.get() + max_num_nonzeros_, new_values); - values_.reset(new_values); - - max_num_nonzeros_ = new_max_num_nonzeros; + if (cols_.size() < num_nonzeros() + m.num_nonzeros()) { + cols_.resize(num_nonzeros() + m.num_nonzeros()); + values_.resize(num_nonzeros() + m.num_nonzeros()); } // Copy the contents of m into this matrix. - copy(m.cols(), m.cols() + m.num_nonzeros(), cols_.get() + num_nonzeros()); - copy(m.values(), - m.values() + m.num_nonzeros(), - values_.get() + num_nonzeros()); - - // Create the new rows array to hold the enlarged matrix. - int* new_rows = new int[num_rows_ + m.num_rows() + 1]; - // The first num_rows_ entries are the same - copy(rows_.get(), rows_.get() + num_rows_, new_rows); - + copy(m.cols(), m.cols() + m.num_nonzeros(), &cols_[num_nonzeros()]); + copy(m.values(), m.values() + m.num_nonzeros(), &values_[num_nonzeros()]); + rows_.resize(num_rows_ + m.num_rows() + 1); // new_rows = [rows_, m.row() + rows_[num_rows_]] - fill(new_rows + num_rows_, - new_rows + num_rows_ + m.num_rows() + 1, + fill(rows_.begin() + num_rows_, + rows_.begin() + num_rows_ + m.num_rows() + 1, rows_[num_rows_]); for (int r = 0; r < m.num_rows() + 1; ++r) { - new_rows[num_rows_ + r] += m.rows()[r]; + rows_[num_rows_ + r] += m.rows()[r]; } - rows_.reset(new_rows); num_rows_ += m.num_rows(); } @@ -332,23 +245,122 @@ void CompressedRowSparseMatrix::ToTextFile(FILE* file) const { CHECK_NOTNULL(file); for (int r = 0; r < num_rows_; ++r) { for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { - fprintf(file, "% 10d % 10d %17f\n", r, cols_[idx], values_[idx]); + fprintf(file, + "% 10d % 10d %17f\n", + r, + cols_[idx], + values_[idx]); } } } void CompressedRowSparseMatrix::ToCRSMatrix(CRSMatrix* matrix) const { - matrix->num_rows = num_rows(); - matrix->num_cols = num_cols(); + matrix->num_rows = num_rows_; + matrix->num_cols = num_cols_; + matrix->rows = rows_; + matrix->cols = cols_; + matrix->values = values_; + // Trim. matrix->rows.resize(matrix->num_rows + 1); - matrix->cols.resize(num_nonzeros()); - matrix->values.resize(num_nonzeros()); + matrix->cols.resize(matrix->rows[matrix->num_rows]); + matrix->values.resize(matrix->rows[matrix->num_rows]); +} - 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()); +void CompressedRowSparseMatrix::SolveLowerTriangularInPlace( + double* solution) const { + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1] - 1; ++idx) { + solution[r] -= values_[idx] * solution[cols_[idx]]; + } + solution[r] /= values_[rows_[r + 1] - 1]; + } } +void CompressedRowSparseMatrix::SolveLowerTriangularTransposeInPlace( + double* solution) const { + for (int r = num_rows_ - 1; r >= 0; --r) { + solution[r] /= values_[rows_[r + 1] - 1]; + for (int idx = rows_[r + 1] - 2; idx >= rows_[r]; --idx) { + solution[cols_[idx]] -= values_[idx] * solution[r]; + } + } +} + +CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( + const double* diagonal, + const vector<int>& blocks) { + int num_rows = 0; + int num_nonzeros = 0; + for (int i = 0; i < blocks.size(); ++i) { + num_rows += blocks[i]; + num_nonzeros += blocks[i] * blocks[i]; + } + + CompressedRowSparseMatrix* matrix = + new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros); + + int* rows = matrix->mutable_rows(); + int* cols = matrix->mutable_cols(); + double* values = matrix->mutable_values(); + fill(values, values + num_nonzeros, 0.0); + + int idx_cursor = 0; + int col_cursor = 0; + for (int i = 0; i < blocks.size(); ++i) { + const int block_size = blocks[i]; + for (int r = 0; r < block_size; ++r) { + *(rows++) = idx_cursor; + values[idx_cursor + r] = diagonal[col_cursor + r]; + for (int c = 0; c < block_size; ++c, ++idx_cursor) { + *(cols++) = col_cursor + c; + } + } + col_cursor += block_size; + } + *rows = idx_cursor; + + *matrix->mutable_row_blocks() = blocks; + *matrix->mutable_col_blocks() = blocks; + + CHECK_EQ(idx_cursor, num_nonzeros); + CHECK_EQ(col_cursor, num_rows); + return matrix; +} + +CompressedRowSparseMatrix* CompressedRowSparseMatrix::Transpose() const { + CompressedRowSparseMatrix* transpose = + new CompressedRowSparseMatrix(num_cols_, num_rows_, num_nonzeros()); + + int* transpose_rows = transpose->mutable_rows(); + int* transpose_cols = transpose->mutable_cols(); + double* transpose_values = transpose->mutable_values(); + + for (int idx = 0; idx < num_nonzeros(); ++idx) { + ++transpose_rows[cols_[idx] + 1]; + } + + for (int i = 1; i < transpose->num_rows() + 1; ++i) { + transpose_rows[i] += transpose_rows[i - 1]; + } + + for (int r = 0; r < num_rows(); ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + const int c = cols_[idx]; + const int transpose_idx = transpose_rows[c]++; + transpose_cols[transpose_idx] = r; + transpose_values[transpose_idx] = values_[idx]; + } + } + + for (int i = transpose->num_rows() - 1; i > 0 ; --i) { + transpose_rows[i] = transpose_rows[i - 1]; + } + transpose_rows[0] = 0; + + return transpose; +} + + } // 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 c9c904bf63c..c5721eb888a 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 @@ -32,12 +32,9 @@ #define CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ #include <vector> - -#include "ceres/internal/eigen.h" #include "ceres/internal/macros.h" #include "ceres/internal/port.h" #include "ceres/sparse_matrix.h" -#include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" #include "glog/logging.h" @@ -47,7 +44,7 @@ struct CRSMatrix; namespace internal { -class SparseMatrixProto; +class TripletSparseMatrix; class CompressedRowSparseMatrix : public SparseMatrix { public: @@ -58,9 +55,6 @@ class CompressedRowSparseMatrix : public SparseMatrix { // // We assume that m does not have any repeated entries. explicit CompressedRowSparseMatrix(const TripletSparseMatrix& m); -#ifndef CERES_NO_PROTOCOL_BUFFERS - explicit CompressedRowSparseMatrix(const SparseMatrixProto& proto); -#endif // Use this constructor only if you know what you are doing. This // creates a "blank" matrix with the appropriate amount of memory @@ -91,15 +85,12 @@ class CompressedRowSparseMatrix : public SparseMatrix { virtual void ScaleColumns(const double* scale); virtual void ToDenseMatrix(Matrix* dense_matrix) const; -#ifndef CERES_NO_PROTOCOL_BUFFERS - virtual void ToProto(SparseMatrixProto* proto) const; -#endif virtual void ToTextFile(FILE* file) const; virtual int num_rows() const { return num_rows_; } virtual int num_cols() const { return num_cols_; } virtual int num_nonzeros() const { return rows_[num_rows_]; } - virtual const double* values() const { return values_.get(); } - virtual double* mutable_values() { return values_.get(); } + virtual const double* values() const { return &values_[0]; } + virtual double* mutable_values() { return &values_[0]; } // Delete the bottom delta_rows. // num_rows -= delta_rows @@ -112,11 +103,11 @@ class CompressedRowSparseMatrix : public SparseMatrix { 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(); } + const int* cols() const { return &cols_[0]; } + int* mutable_cols() { return &cols_[0]; } - const int* rows() const { return rows_.get(); } - int* mutable_rows() { return rows_.get(); } + const int* rows() const { return &rows_[0]; } + int* mutable_rows() { return &rows_[0]; } const vector<int>& row_blocks() const { return row_blocks_; } vector<int>* mutable_row_blocks() { return &row_blocks_; } @@ -124,14 +115,25 @@ class CompressedRowSparseMatrix : public SparseMatrix { 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_; - scoped_array<double> values_; + // Non-destructive array resizing method. + void set_num_rows(const int num_rows) { num_rows_ = num_rows; } + void set_num_cols(const int num_cols) { num_cols_ = num_cols; } + + void SolveLowerTriangularInPlace(double* solution) const; + void SolveLowerTriangularTransposeInPlace(double* solution) const; + CompressedRowSparseMatrix* Transpose() const; + + static CompressedRowSparseMatrix* CreateBlockDiagonalMatrix( + const double* diagonal, + const vector<int>& blocks); + + private: int num_rows_; int num_cols_; - int max_num_nonzeros_; + vector<int> rows_; + vector<int> cols_; + vector<double> values_; // If the matrix has an underlying block structure, then it can also // carry with it row and column block sizes. This is auxilliary and diff --git a/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.h b/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.h index 3dcf8faee59..424acda94ae 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.h +++ b/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.h @@ -31,6 +31,7 @@ #ifndef CERES_INTERNAL_COORDINATE_DESCENT_MINIMIZER_H_ #define CERES_INTERNAL_COORDINATE_DESCENT_MINIMIZER_H_ +#include <string> #include <vector> #include "ceres/evaluator.h" diff --git a/extern/libmv/third_party/ceres/internal/ceres/corrector.cc b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc index c3858abd2f4..60269a6a4b9 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/corrector.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc @@ -32,7 +32,6 @@ #include <cstddef> #include <cmath> -#include "ceres/internal/eigen.h" #include "glog/logging.h" namespace ceres { @@ -90,7 +89,7 @@ Corrector::Corrector(double sq_norm, const double rho[3]) { // 0.5 * alpha^2 - alpha - rho'' / rho' * z'z = 0. // // Start by calculating the discriminant D. - const double D = 1.0 + 2.0 * sq_norm*rho[2] / rho[1]; + const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1]; // Since both rho[1] and rho[2] are guaranteed to be positive at // this point, we know that D > 1.0. @@ -102,29 +101,43 @@ Corrector::Corrector(double sq_norm, const double rho[3]) { alpha_sq_norm_ = alpha / sq_norm; } -void Corrector::CorrectResiduals(int nrow, double* residuals) { +void Corrector::CorrectResiduals(int num_rows, double* residuals) { DCHECK(residuals != NULL); - VectorRef r_ref(residuals, nrow); // Equation 11 in BANS. - r_ref *= residual_scaling_; + for (int r = 0; r < num_rows; ++r) { + residuals[r] *= residual_scaling_; + } } -void Corrector::CorrectJacobian(int nrow, int ncol, - double* residuals, double* jacobian) { +void Corrector::CorrectJacobian(int num_rows, + int num_cols, + double* residuals, + double* jacobian) { DCHECK(residuals != NULL); DCHECK(jacobian != NULL); + // Equation 11 in BANS. + // + // J = sqrt(rho) * (J - alpha^2 r * r' J) + // + // In days gone by this loop used to be a single Eigen expression of + // the form + // + // J = sqrt_rho1_ * (J - alpha_sq_norm_ * r* (r.transpose() * J)); + // + // Which turns out to about 17x slower on bal problems. The reason + // is that Eigen is unable to figure out that this expression can be + // evaluated columnwise and ends up creating a temporary. + for (int c = 0; c < num_cols; ++c) { + double r_transpose_j = 0.0; + for (int r = 0; r < num_rows; ++r) { + r_transpose_j += jacobian[r * num_cols + c] * residuals[r]; + } - if (nrow == 1) { - // Specialization for the case where the residual is a scalar. - VectorRef j_ref(jacobian, ncol); - j_ref *= sqrt_rho1_ * (1.0 - alpha_sq_norm_ * pow(*residuals, 2)); - } else { - ConstVectorRef r_ref(residuals, nrow); - MatrixRef j_ref(jacobian, nrow, ncol); - - // Equation 11 in BANS. - j_ref = sqrt_rho1_ * (j_ref - alpha_sq_norm_ * - r_ref * (r_ref.transpose() * j_ref)); + for (int r = 0; r < num_rows; ++r) { + jacobian[r * num_cols + c] = sqrt_rho1_ * + (jacobian[r * num_cols + c] - + alpha_sq_norm_ * residuals[r] * r_transpose_j); + } } } diff --git a/extern/libmv/third_party/ceres/internal/ceres/corrector.h b/extern/libmv/third_party/ceres/internal/ceres/corrector.h index 9914641cb01..2137221784e 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/corrector.h +++ b/extern/libmv/third_party/ceres/internal/ceres/corrector.h @@ -66,7 +66,7 @@ class Corrector { explicit Corrector(double sq_norm, const double rho[3]); // residuals *= sqrt(rho[1]) / (1 - alpha) - void CorrectResiduals(int nrow, double* residuals); + void CorrectResiduals(int num_rows, double* residuals); // jacobian = sqrt(rho[1]) * jacobian - // sqrt(rho[1]) * alpha / sq_norm * residuals residuals' * jacobian. @@ -74,8 +74,10 @@ class Corrector { // The method assumes that the jacobian has row-major storage. It is // the caller's responsibility to ensure that the pointer to // jacobian is not null. - void CorrectJacobian(int nrow, int ncol, - double* residuals, double* jacobian); + void CorrectJacobian(int num_rows, + int num_cols, + double* residuals, + double* jacobian); private: double sqrt_rho1_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h b/extern/libmv/third_party/ceres/internal/ceres/covariance.cc index 94b3076e3d7..35146c582b2 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h +++ b/extern/libmv/third_party/ceres/internal/ceres/covariance.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// Copyright 2013 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -26,15 +26,37 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. // -// Author: keir@google.com (Keir Mierle) -// -// A portability header to make optional protocol buffer support less intrusive. +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/covariance.h" + +#include <utility> +#include <vector> +#include "ceres/covariance_impl.h" +#include "ceres/problem.h" +#include "ceres/problem_impl.h" + +namespace ceres { + +Covariance::Covariance(const Covariance::Options& options) { + impl_.reset(new internal::CovarianceImpl(options)); +} + +Covariance::~Covariance() { +} -#ifndef CERES_INTERNAL_MATRIX_PROTO_H_ -#define CERES_INTERNAL_MATRIX_PROTO_H_ +bool Covariance::Compute( + const vector<pair<const double*, const double*> >& covariance_blocks, + Problem* problem) { + return impl_->Compute(covariance_blocks, problem->problem_impl_.get()); +} -#ifndef CERES_NO_PROTOCOL_BUFFERS -#include "ceres/matrix.pb.h" -#endif +bool Covariance::GetCovarianceBlock(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const { + return impl_->GetCovarianceBlock(parameter_block1, + parameter_block2, + covariance_block); +} -#endif // CERES_INTERNAL_MATRIX_PROTO_H_ +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.cc new file mode 100644 index 00000000000..19d545cc2d3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.cc @@ -0,0 +1,845 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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/covariance_impl.h" + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +#include <algorithm> +#include <utility> +#include <vector> +#include "Eigen/SVD" +#include "ceres/compressed_col_sparse_matrix_utils.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/covariance.h" +#include "ceres/crs_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/map_util.h" +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/suitesparse.h" +#include "ceres/wall_time.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { + +// Per thread storage for SuiteSparse. +#ifndef CERES_NO_SUITESPARSE + +struct PerThreadContext { + explicit PerThreadContext(int num_rows) + : solution(NULL), + solution_set(NULL), + y_workspace(NULL), + e_workspace(NULL), + rhs(NULL) { + rhs = ss.CreateDenseVector(NULL, num_rows, num_rows); + } + + ~PerThreadContext() { + ss.Free(solution); + ss.Free(solution_set); + ss.Free(y_workspace); + ss.Free(e_workspace); + ss.Free(rhs); + } + + cholmod_dense* solution; + cholmod_sparse* solution_set; + cholmod_dense* y_workspace; + cholmod_dense* e_workspace; + cholmod_dense* rhs; + SuiteSparse ss; +}; + +#endif + +} // namespace + +typedef vector<pair<const double*, const double*> > CovarianceBlocks; + +CovarianceImpl::CovarianceImpl(const Covariance::Options& options) + : options_(options), + is_computed_(false), + is_valid_(false) { + evaluate_options_.num_threads = options.num_threads; + evaluate_options_.apply_loss_function = options.apply_loss_function; +} + +CovarianceImpl::~CovarianceImpl() { +} + +bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks, + ProblemImpl* problem) { + problem_ = problem; + parameter_block_to_row_index_.clear(); + covariance_matrix_.reset(NULL); + is_valid_ = (ComputeCovarianceSparsity(covariance_blocks, problem) && + ComputeCovarianceValues()); + is_computed_ = true; + return is_valid_; +} + +bool CovarianceImpl::GetCovarianceBlock(const double* original_parameter_block1, + const double* original_parameter_block2, + double* covariance_block) const { + CHECK(is_computed_) + << "Covariance::GetCovarianceBlock called before Covariance::Compute"; + CHECK(is_valid_) + << "Covariance::GetCovarianceBlock called when Covariance::Compute " + << "returned false."; + + // If either of the two parameter blocks is constant, then the + // covariance block is also zero. + if (constant_parameter_blocks_.count(original_parameter_block1) > 0 || + constant_parameter_blocks_.count(original_parameter_block2) > 0) { + const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map(); + ParameterBlock* block1 = + FindOrDie(parameter_map, + const_cast<double*>(original_parameter_block1)); + + ParameterBlock* block2 = + FindOrDie(parameter_map, + const_cast<double*>(original_parameter_block2)); + const int block1_size = block1->Size(); + const int block2_size = block2->Size(); + MatrixRef(covariance_block, block1_size, block2_size).setZero(); + return true; + } + + const double* parameter_block1 = original_parameter_block1; + const double* parameter_block2 = original_parameter_block2; + const bool transpose = parameter_block1 > parameter_block2; + if (transpose) { + std::swap(parameter_block1, parameter_block2); + } + + // Find where in the covariance matrix the block is located. + const int row_begin = + FindOrDie(parameter_block_to_row_index_, parameter_block1); + const int col_begin = + FindOrDie(parameter_block_to_row_index_, parameter_block2); + const int* rows = covariance_matrix_->rows(); + const int* cols = covariance_matrix_->cols(); + const int row_size = rows[row_begin + 1] - rows[row_begin]; + const int* cols_begin = cols + rows[row_begin]; + + // The only part that requires work is walking the compressed column + // vector to determine where the set of columns correspnding to the + // covariance block begin. + int offset = 0; + while (cols_begin[offset] != col_begin && offset < row_size) { + ++offset; + } + + if (offset == row_size) { + LOG(WARNING) << "Unable to find covariance block for " + << original_parameter_block1 << " " + << original_parameter_block2; + return false; + } + + const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map(); + ParameterBlock* block1 = + FindOrDie(parameter_map, const_cast<double*>(parameter_block1)); + ParameterBlock* block2 = + FindOrDie(parameter_map, const_cast<double*>(parameter_block2)); + const LocalParameterization* local_param1 = block1->local_parameterization(); + const LocalParameterization* local_param2 = block2->local_parameterization(); + const int block1_size = block1->Size(); + const int block1_local_size = block1->LocalSize(); + const int block2_size = block2->Size(); + const int block2_local_size = block2->LocalSize(); + + ConstMatrixRef cov(covariance_matrix_->values() + rows[row_begin], + block1_size, + row_size); + + // Fast path when there are no local parameterizations. + if (local_param1 == NULL && local_param2 == NULL) { + if (transpose) { + MatrixRef(covariance_block, block2_size, block1_size) = + cov.block(0, offset, block1_size, block2_size).transpose(); + } else { + MatrixRef(covariance_block, block1_size, block2_size) = + cov.block(0, offset, block1_size, block2_size); + } + return true; + } + + // If local parameterizations are used then the covariance that has + // been computed is in the tangent space and it needs to be lifted + // back to the ambient space. + // + // This is given by the formula + // + // C'_12 = J_1 C_12 J_2' + // + // Where C_12 is the local tangent space covariance for parameter + // blocks 1 and 2. J_1 and J_2 are respectively the local to global + // jacobians for parameter blocks 1 and 2. + // + // See Result 5.11 on page 142 of Hartley & Zisserman (2nd Edition) + // for a proof. + // + // TODO(sameeragarwal): Add caching of local parameterization, so + // that they are computed just once per parameter block. + Matrix block1_jacobian(block1_size, block1_local_size); + if (local_param1 == NULL) { + block1_jacobian.setIdentity(); + } else { + local_param1->ComputeJacobian(parameter_block1, block1_jacobian.data()); + } + + Matrix block2_jacobian(block2_size, block2_local_size); + // Fast path if the user is requesting a diagonal block. + if (parameter_block1 == parameter_block2) { + block2_jacobian = block1_jacobian; + } else { + if (local_param2 == NULL) { + block2_jacobian.setIdentity(); + } else { + local_param2->ComputeJacobian(parameter_block2, block2_jacobian.data()); + } + } + + if (transpose) { + MatrixRef(covariance_block, block2_size, block1_size) = + block2_jacobian * + cov.block(0, offset, block1_local_size, block2_local_size).transpose() * + block1_jacobian.transpose(); + } else { + MatrixRef(covariance_block, block1_size, block2_size) = + block1_jacobian * + cov.block(0, offset, block1_local_size, block2_local_size) * + block2_jacobian.transpose(); + } + + return true; +} + +// Determine the sparsity pattern of the covariance matrix based on +// the block pairs requested by the user. +bool CovarianceImpl::ComputeCovarianceSparsity( + const CovarianceBlocks& original_covariance_blocks, + ProblemImpl* problem) { + EventLogger event_logger("CovarianceImpl::ComputeCovarianceSparsity"); + + // Determine an ordering for the parameter block, by sorting the + // parameter blocks by their pointers. + vector<double*> all_parameter_blocks; + problem->GetParameterBlocks(&all_parameter_blocks); + const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map(); + constant_parameter_blocks_.clear(); + vector<double*>& active_parameter_blocks = evaluate_options_.parameter_blocks; + active_parameter_blocks.clear(); + for (int i = 0; i < all_parameter_blocks.size(); ++i) { + double* parameter_block = all_parameter_blocks[i]; + + ParameterBlock* block = FindOrDie(parameter_map, parameter_block); + if (block->IsConstant()) { + constant_parameter_blocks_.insert(parameter_block); + } else { + active_parameter_blocks.push_back(parameter_block); + } + } + + sort(active_parameter_blocks.begin(), active_parameter_blocks.end()); + + // Compute the number of rows. Map each parameter block to the + // first row corresponding to it in the covariance matrix using the + // ordering of parameter blocks just constructed. + int num_rows = 0; + parameter_block_to_row_index_.clear(); + for (int i = 0; i < active_parameter_blocks.size(); ++i) { + double* parameter_block = active_parameter_blocks[i]; + const int parameter_block_size = + problem->ParameterBlockLocalSize(parameter_block); + parameter_block_to_row_index_[parameter_block] = num_rows; + num_rows += parameter_block_size; + } + + // Compute the number of non-zeros in the covariance matrix. Along + // the way flip any covariance blocks which are in the lower + // triangular part of the matrix. + int num_nonzeros = 0; + CovarianceBlocks covariance_blocks; + for (int i = 0; i < original_covariance_blocks.size(); ++i) { + const pair<const double*, const double*>& block_pair = + original_covariance_blocks[i]; + if (constant_parameter_blocks_.count(block_pair.first) > 0 || + constant_parameter_blocks_.count(block_pair.second) > 0) { + continue; + } + + int index1 = FindOrDie(parameter_block_to_row_index_, block_pair.first); + int index2 = FindOrDie(parameter_block_to_row_index_, block_pair.second); + const int size1 = problem->ParameterBlockLocalSize(block_pair.first); + const int size2 = problem->ParameterBlockLocalSize(block_pair.second); + num_nonzeros += size1 * size2; + + // Make sure we are constructing a block upper triangular matrix. + if (index1 > index2) { + covariance_blocks.push_back(make_pair(block_pair.second, + block_pair.first)); + } else { + covariance_blocks.push_back(block_pair); + } + } + + if (covariance_blocks.size() == 0) { + VLOG(2) << "No non-zero covariance blocks found"; + covariance_matrix_.reset(NULL); + return true; + } + + // Sort the block pairs. As a consequence we get the covariance + // blocks as they will occur in the CompressedRowSparseMatrix that + // will store the covariance. + sort(covariance_blocks.begin(), covariance_blocks.end()); + + // Fill the sparsity pattern of the covariance matrix. + covariance_matrix_.reset( + new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros)); + + int* rows = covariance_matrix_->mutable_rows(); + int* cols = covariance_matrix_->mutable_cols(); + + // Iterate over parameter blocks and in turn over the rows of the + // covariance matrix. For each parameter block, look in the upper + // triangular part of the covariance matrix to see if there are any + // blocks requested by the user. If this is the case then fill out a + // set of compressed rows corresponding to this parameter block. + // + // The key thing that makes this loop work is the fact that the + // row/columns of the covariance matrix are ordered by the pointer + // values of the parameter blocks. Thus iterating over the keys of + // parameter_block_to_row_index_ corresponds to iterating over the + // rows of the covariance matrix in order. + int i = 0; // index into covariance_blocks. + int cursor = 0; // index into the covariance matrix. + for (map<const double*, int>::const_iterator it = + parameter_block_to_row_index_.begin(); + it != parameter_block_to_row_index_.end(); + ++it) { + const double* row_block = it->first; + const int row_block_size = problem->ParameterBlockLocalSize(row_block); + int row_begin = it->second; + + // Iterate over the covariance blocks contained in this row block + // and count the number of columns in this row block. + int num_col_blocks = 0; + int num_columns = 0; + for (int j = i; j < covariance_blocks.size(); ++j, ++num_col_blocks) { + const pair<const double*, const double*>& block_pair = + covariance_blocks[j]; + if (block_pair.first != row_block) { + break; + } + num_columns += problem->ParameterBlockLocalSize(block_pair.second); + } + + // Fill out all the compressed rows for this parameter block. + for (int r = 0; r < row_block_size; ++r) { + rows[row_begin + r] = cursor; + for (int c = 0; c < num_col_blocks; ++c) { + const double* col_block = covariance_blocks[i + c].second; + const int col_block_size = problem->ParameterBlockLocalSize(col_block); + int col_begin = FindOrDie(parameter_block_to_row_index_, col_block); + for (int k = 0; k < col_block_size; ++k) { + cols[cursor++] = col_begin++; + } + } + } + + i+= num_col_blocks; + } + + rows[num_rows] = cursor; + return true; +} + +bool CovarianceImpl::ComputeCovarianceValues() { + switch (options_.algorithm_type) { + case (DENSE_SVD): + return ComputeCovarianceValuesUsingDenseSVD(); +#ifndef CERES_NO_SUITESPARSE + case (SPARSE_CHOLESKY): + return ComputeCovarianceValuesUsingSparseCholesky(); + case (SPARSE_QR): + return ComputeCovarianceValuesUsingSparseQR(); +#endif + default: + LOG(ERROR) << "Unsupported covariance estimation algorithm type: " + << CovarianceAlgorithmTypeToString(options_.algorithm_type); + return false; + } + return false; +} + +bool CovarianceImpl::ComputeCovarianceValuesUsingSparseCholesky() { + EventLogger event_logger( + "CovarianceImpl::ComputeCovarianceValuesUsingSparseCholesky"); +#ifndef CERES_NO_SUITESPARSE + if (covariance_matrix_.get() == NULL) { + // Nothing to do, all zeros covariance matrix. + return true; + } + + SuiteSparse ss; + + CRSMatrix jacobian; + problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian); + + event_logger.AddEvent("Evaluate"); + // m is a transposed view of the Jacobian. + cholmod_sparse cholmod_jacobian_view; + cholmod_jacobian_view.nrow = jacobian.num_cols; + cholmod_jacobian_view.ncol = jacobian.num_rows; + cholmod_jacobian_view.nzmax = jacobian.values.size(); + cholmod_jacobian_view.nz = NULL; + cholmod_jacobian_view.p = reinterpret_cast<void*>(&jacobian.rows[0]); + cholmod_jacobian_view.i = reinterpret_cast<void*>(&jacobian.cols[0]); + cholmod_jacobian_view.x = reinterpret_cast<void*>(&jacobian.values[0]); + cholmod_jacobian_view.z = NULL; + cholmod_jacobian_view.stype = 0; // Matrix is not symmetric. + cholmod_jacobian_view.itype = CHOLMOD_INT; + cholmod_jacobian_view.xtype = CHOLMOD_REAL; + cholmod_jacobian_view.dtype = CHOLMOD_DOUBLE; + cholmod_jacobian_view.sorted = 1; + cholmod_jacobian_view.packed = 1; + + cholmod_factor* factor = ss.AnalyzeCholesky(&cholmod_jacobian_view); + event_logger.AddEvent("Symbolic Factorization"); + bool factorization_succeeded = ss.Cholesky(&cholmod_jacobian_view, factor); + if (factorization_succeeded) { + const double reciprocal_condition_number = + cholmod_rcond(factor, ss.mutable_cc()); + if (reciprocal_condition_number < + options_.min_reciprocal_condition_number) { + LOG(WARNING) << "Cholesky factorization of J'J is not reliable. " + << "Reciprocal condition number: " + << reciprocal_condition_number << " " + << "min_reciprocal_condition_number : " + << options_.min_reciprocal_condition_number; + factorization_succeeded = false; + } + } + + event_logger.AddEvent("Numeric Factorization"); + if (!factorization_succeeded) { + ss.Free(factor); + LOG(WARNING) << "Cholesky factorization failed."; + return false; + } + + const int num_rows = covariance_matrix_->num_rows(); + const int* rows = covariance_matrix_->rows(); + const int* cols = covariance_matrix_->cols(); + double* values = covariance_matrix_->mutable_values(); + + // The following loop exploits the fact that the i^th column of A^{-1} + // is given by the solution to the linear system + // + // A x = e_i + // + // where e_i is a vector with e(i) = 1 and all other entries zero. + // + // Since the covariance matrix is symmetric, the i^th row and column + // are equal. + // + // The ifdef separates two different version of SuiteSparse. Newer + // versions of SuiteSparse have the cholmod_solve2 function which + // re-uses memory across calls. +#if (SUITESPARSE_VERSION < 4002) + cholmod_dense* rhs = ss.CreateDenseVector(NULL, num_rows, num_rows); + double* rhs_x = reinterpret_cast<double*>(rhs->x); + + for (int r = 0; r < num_rows; ++r) { + int row_begin = rows[r]; + int row_end = rows[r + 1]; + if (row_end == row_begin) { + continue; + } + + rhs_x[r] = 1.0; + cholmod_dense* solution = ss.Solve(factor, rhs); + double* solution_x = reinterpret_cast<double*>(solution->x); + for (int idx = row_begin; idx < row_end; ++idx) { + const int c = cols[idx]; + values[idx] = solution_x[c]; + } + ss.Free(solution); + rhs_x[r] = 0.0; + } + + ss.Free(rhs); +#else // SUITESPARSE_VERSION < 4002 + + const int num_threads = options_.num_threads; + vector<PerThreadContext*> contexts(num_threads); + for (int i = 0; i < num_threads; ++i) { + contexts[i] = new PerThreadContext(num_rows); + } + + // The first call to cholmod_solve2 is not thread safe, since it + // changes the factorization from supernodal to simplicial etc. + { + PerThreadContext* context = contexts[0]; + double* context_rhs_x = reinterpret_cast<double*>(context->rhs->x); + context_rhs_x[0] = 1.0; + cholmod_solve2(CHOLMOD_A, + factor, + context->rhs, + NULL, + &context->solution, + &context->solution_set, + &context->y_workspace, + &context->e_workspace, + context->ss.mutable_cc()); + context_rhs_x[0] = 0.0; + } + +#pragma omp parallel for num_threads(num_threads) schedule(dynamic) + for (int r = 0; r < num_rows; ++r) { + int row_begin = rows[r]; + int row_end = rows[r + 1]; + if (row_end == row_begin) { + continue; + } + +# ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +# else + int thread_id = 0; +# endif + + PerThreadContext* context = contexts[thread_id]; + double* context_rhs_x = reinterpret_cast<double*>(context->rhs->x); + context_rhs_x[r] = 1.0; + + // TODO(sameeragarwal) There should be a more efficient way + // involving the use of Bset but I am unable to make it work right + // now. + cholmod_solve2(CHOLMOD_A, + factor, + context->rhs, + NULL, + &context->solution, + &context->solution_set, + &context->y_workspace, + &context->e_workspace, + context->ss.mutable_cc()); + + double* solution_x = reinterpret_cast<double*>(context->solution->x); + for (int idx = row_begin; idx < row_end; ++idx) { + const int c = cols[idx]; + values[idx] = solution_x[c]; + } + context_rhs_x[r] = 0.0; + } + + for (int i = 0; i < num_threads; ++i) { + delete contexts[i]; + } + +#endif // SUITESPARSE_VERSION < 4002 + + ss.Free(factor); + event_logger.AddEvent("Inversion"); + return true; + +#else // CERES_NO_SUITESPARSE + + return false; + +#endif // CERES_NO_SUITESPARSE +}; + +bool CovarianceImpl::ComputeCovarianceValuesUsingSparseQR() { + EventLogger event_logger( + "CovarianceImpl::ComputeCovarianceValuesUsingSparseQR"); + +#ifndef CERES_NO_SUITESPARSE + if (covariance_matrix_.get() == NULL) { + // Nothing to do, all zeros covariance matrix. + return true; + } + + CRSMatrix jacobian; + problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian); + event_logger.AddEvent("Evaluate"); + + // Construct a compressed column form of the Jacobian. + const int num_rows = jacobian.num_rows; + const int num_cols = jacobian.num_cols; + const int num_nonzeros = jacobian.values.size(); + + vector<SuiteSparse_long> transpose_rows(num_cols + 1, 0); + vector<SuiteSparse_long> transpose_cols(num_nonzeros, 0); + vector<double> transpose_values(num_nonzeros, 0); + + for (int idx = 0; idx < num_nonzeros; ++idx) { + transpose_rows[jacobian.cols[idx] + 1] += 1; + } + + for (int i = 1; i < transpose_rows.size(); ++i) { + transpose_rows[i] += transpose_rows[i - 1]; + } + + for (int r = 0; r < num_rows; ++r) { + for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) { + const int c = jacobian.cols[idx]; + const int transpose_idx = transpose_rows[c]; + transpose_cols[transpose_idx] = r; + transpose_values[transpose_idx] = jacobian.values[idx]; + ++transpose_rows[c]; + } + } + + for (int i = transpose_rows.size() - 1; i > 0 ; --i) { + transpose_rows[i] = transpose_rows[i - 1]; + } + transpose_rows[0] = 0; + + cholmod_sparse cholmod_jacobian; + cholmod_jacobian.nrow = num_rows; + cholmod_jacobian.ncol = num_cols; + cholmod_jacobian.nzmax = num_nonzeros; + cholmod_jacobian.nz = NULL; + cholmod_jacobian.p = reinterpret_cast<void*>(&transpose_rows[0]); + cholmod_jacobian.i = reinterpret_cast<void*>(&transpose_cols[0]); + cholmod_jacobian.x = reinterpret_cast<void*>(&transpose_values[0]); + cholmod_jacobian.z = NULL; + cholmod_jacobian.stype = 0; // Matrix is not symmetric. + cholmod_jacobian.itype = CHOLMOD_LONG; + cholmod_jacobian.xtype = CHOLMOD_REAL; + cholmod_jacobian.dtype = CHOLMOD_DOUBLE; + cholmod_jacobian.sorted = 1; + cholmod_jacobian.packed = 1; + + cholmod_common cc; + cholmod_l_start(&cc); + + cholmod_sparse* R = NULL; + SuiteSparse_long* permutation = NULL; + + // Compute a Q-less QR factorization of the Jacobian. Since we are + // only interested in inverting J'J = R'R, we do not need Q. This + // saves memory and gives us R as a permuted compressed column + // sparse matrix. + // + // TODO(sameeragarwal): Currently the symbolic factorization and the + // numeric factorization is done at the same time, and this does not + // explicitly account for the block column and row structure in the + // matrix. When using AMD, we have observed in the past that + // computing the ordering with the block matrix is significantly + // more efficient, both in runtime as well as the quality of + // ordering computed. So, it maybe worth doing that analysis + // separately. + const SuiteSparse_long rank = + SuiteSparseQR<double>(SPQR_ORDERING_BESTAMD, + SPQR_DEFAULT_TOL, + cholmod_jacobian.ncol, + &cholmod_jacobian, + &R, + &permutation, + &cc); + event_logger.AddEvent("Numeric Factorization"); + CHECK_NOTNULL(permutation); + CHECK_NOTNULL(R); + + if (rank < cholmod_jacobian.ncol) { + LOG(WARNING) << "Jacobian matrix is rank deficient." + << "Number of columns: " << cholmod_jacobian.ncol + << " rank: " << rank; + delete []permutation; + cholmod_l_free_sparse(&R, &cc); + cholmod_l_finish(&cc); + return false; + } + + vector<int> inverse_permutation(num_cols); + for (SuiteSparse_long i = 0; i < num_cols; ++i) { + inverse_permutation[permutation[i]] = i; + } + + const int* rows = covariance_matrix_->rows(); + const int* cols = covariance_matrix_->cols(); + double* values = covariance_matrix_->mutable_values(); + + // The following loop exploits the fact that the i^th column of A^{-1} + // is given by the solution to the linear system + // + // A x = e_i + // + // where e_i is a vector with e(i) = 1 and all other entries zero. + // + // Since the covariance matrix is symmetric, the i^th row and column + // are equal. + const int num_threads = options_.num_threads; + scoped_array<double> workspace(new double[num_threads * num_cols]); + +#pragma omp parallel for num_threads(num_threads) schedule(dynamic) + for (int r = 0; r < num_cols; ++r) { + const int row_begin = rows[r]; + const int row_end = rows[r + 1]; + if (row_end == row_begin) { + continue; + } + +# ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +# else + int thread_id = 0; +# endif + + double* solution = workspace.get() + thread_id * num_cols; + SolveRTRWithSparseRHS<SuiteSparse_long>( + num_cols, + static_cast<SuiteSparse_long*>(R->i), + static_cast<SuiteSparse_long*>(R->p), + static_cast<double*>(R->x), + inverse_permutation[r], + solution); + for (int idx = row_begin; idx < row_end; ++idx) { + const int c = cols[idx]; + values[idx] = solution[inverse_permutation[c]]; + } + } + + delete []permutation; + cholmod_l_free_sparse(&R, &cc); + cholmod_l_finish(&cc); + event_logger.AddEvent("Inversion"); + return true; + +#else // CERES_NO_SUITESPARSE + + return false; + +#endif // CERES_NO_SUITESPARSE +} + +bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() { + EventLogger event_logger( + "CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD"); + if (covariance_matrix_.get() == NULL) { + // Nothing to do, all zeros covariance matrix. + return true; + } + + CRSMatrix jacobian; + problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian); + event_logger.AddEvent("Evaluate"); + + Matrix dense_jacobian(jacobian.num_rows, jacobian.num_cols); + dense_jacobian.setZero(); + for (int r = 0; r < jacobian.num_rows; ++r) { + for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) { + const int c = jacobian.cols[idx]; + dense_jacobian(r, c) = jacobian.values[idx]; + } + } + event_logger.AddEvent("ConvertToDenseMatrix"); + + Eigen::JacobiSVD<Matrix> svd(dense_jacobian, + Eigen::ComputeThinU | Eigen::ComputeThinV); + + event_logger.AddEvent("SingularValueDecomposition"); + + const Vector singular_values = svd.singularValues(); + const int num_singular_values = singular_values.rows(); + Vector inverse_squared_singular_values(num_singular_values); + inverse_squared_singular_values.setZero(); + + const double max_singular_value = singular_values[0]; + const double min_singular_value_ratio = + sqrt(options_.min_reciprocal_condition_number); + + const bool automatic_truncation = (options_.null_space_rank < 0); + const int max_rank = min(num_singular_values, + num_singular_values - options_.null_space_rank); + + // Compute the squared inverse of the singular values. Truncate the + // computation based on min_singular_value_ratio and + // null_space_rank. When either of these two quantities are active, + // the resulting covariance matrix is a Moore-Penrose inverse + // instead of a regular inverse. + for (int i = 0; i < max_rank; ++i) { + const double singular_value_ratio = singular_values[i] / max_singular_value; + if (singular_value_ratio < min_singular_value_ratio) { + // Since the singular values are in decreasing order, if + // automatic truncation is enabled, then from this point on + // all values will fail the ratio test and there is nothing to + // do in this loop. + if (automatic_truncation) { + break; + } else { + LOG(WARNING) << "Cholesky factorization of J'J is not reliable. " + << "Reciprocal condition number: " + << singular_value_ratio * singular_value_ratio << " " + << "min_reciprocal_condition_number : " + << options_.min_reciprocal_condition_number; + return false; + } + } + + inverse_squared_singular_values[i] = + 1.0 / (singular_values[i] * singular_values[i]); + } + + Matrix dense_covariance = + svd.matrixV() * + inverse_squared_singular_values.asDiagonal() * + svd.matrixV().transpose(); + event_logger.AddEvent("PseudoInverse"); + + const int num_rows = covariance_matrix_->num_rows(); + const int* rows = covariance_matrix_->rows(); + const int* cols = covariance_matrix_->cols(); + double* values = covariance_matrix_->mutable_values(); + + for (int r = 0; r < num_rows; ++r) { + for (int idx = rows[r]; idx < rows[r + 1]; ++idx) { + const int c = cols[idx]; + values[idx] = dense_covariance(r, c); + } + } + event_logger.AddEvent("CopyToCovarianceMatrix"); + return true; +}; + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.h b/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.h new file mode 100644 index 00000000000..0e7e2173079 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.h @@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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_COVARIANCE_IMPL_H_ +#define CERES_INTERNAL_COVARIANCE_IMPL_H_ + +#include <map> +#include <set> +#include <utility> +#include <vector> +#include "ceres/covariance.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/problem_impl.h" +#include "ceres/suitesparse.h" + +namespace ceres { + +namespace internal { + +class CompressedRowSparseMatrix; + +class CovarianceImpl { + public: + explicit CovarianceImpl(const Covariance::Options& options); + ~CovarianceImpl(); + + bool Compute( + const vector<pair<const double*, const double*> >& covariance_blocks, + ProblemImpl* problem); + + bool GetCovarianceBlock(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const; + + bool ComputeCovarianceSparsity( + const vector<pair<const double*, const double*> >& covariance_blocks, + ProblemImpl* problem); + + bool ComputeCovarianceValues(); + bool ComputeCovarianceValuesUsingSparseCholesky(); + bool ComputeCovarianceValuesUsingSparseQR(); + bool ComputeCovarianceValuesUsingDenseSVD(); + + const CompressedRowSparseMatrix* covariance_matrix() const { + return covariance_matrix_.get(); + } + + private: + ProblemImpl* problem_; + Covariance::Options options_; + Problem::EvaluateOptions evaluate_options_; + bool is_computed_; + bool is_valid_; + map<const double*, int> parameter_block_to_row_index_; + set<const double*> constant_parameter_blocks_; + scoped_ptr<CompressedRowSparseMatrix> covariance_matrix_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COVARIANCE_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.cc b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.cc index 3fbc2717f64..c6d77439653 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.cc @@ -32,7 +32,10 @@ #include "ceres/cxsparse.h" +#include <vector> +#include "ceres/compressed_col_sparse_matrix_utils.h" #include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/internal/port.h" #include "ceres/triplet_sparse_matrix.h" #include "glog/logging.h" @@ -44,46 +47,46 @@ CXSparse::CXSparse() : scratch_(NULL), scratch_size_(0) { CXSparse::~CXSparse() { if (scratch_size_ > 0) { - cs_free(scratch_); + cs_di_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_); + cs_di_free(scratch_); } - scratch_ = reinterpret_cast<CS_ENTRY*>(cs_malloc(A->n, sizeof(CS_ENTRY))); + scratch_ = + reinterpret_cast<CS_ENTRY*>(cs_di_malloc(A->n, sizeof(CS_ENTRY))); scratch_size_ = A->n; } // Solve using Cholesky factorization - csn* numeric_factorization = cs_chol(A, symbolic_factorization); + csn* numeric_factorization = cs_di_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. + // 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); - + cs_di_ipvec(symbolic_factorization->pinv, b, scratch_, A->n); // Set x = L \ x. - cs_lsolve(numeric_factorization->L, scratch_); - + cs_di_lsolve(numeric_factorization->L, scratch_); // Set x = L' \ x. - cs_ltsolve(numeric_factorization->L, scratch_); - + cs_di_ltsolve(numeric_factorization->L, scratch_); // Set b = P' * x. - cs_pvec(symbolic_factorization->pinv, scratch_, b, A->n); + cs_di_pvec(symbolic_factorization->pinv, scratch_, b, A->n); // Free Cholesky factorization. - cs_nfree(numeric_factorization); + cs_di_nfree(numeric_factorization); return true; } @@ -92,6 +95,72 @@ cs_dis* CXSparse::AnalyzeCholesky(cs_di* A) { return cs_schol(1, A); } +cs_dis* CXSparse::AnalyzeCholeskyWithNaturalOrdering(cs_di* A) { + // order = 0 for Natural ordering. + return cs_schol(0, A); +} + +cs_dis* CXSparse::BlockAnalyzeCholesky(cs_di* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks) { + const int num_row_blocks = row_blocks.size(); + const int num_col_blocks = col_blocks.size(); + + vector<int> block_rows; + vector<int> block_cols; + CompressedColumnScalarMatrixToBlockMatrix(A->i, + A->p, + row_blocks, + col_blocks, + &block_rows, + &block_cols); + cs_di block_matrix; + block_matrix.m = num_row_blocks; + block_matrix.n = num_col_blocks; + block_matrix.nz = -1; + block_matrix.nzmax = block_rows.size(); + block_matrix.p = &block_cols[0]; + block_matrix.i = &block_rows[0]; + block_matrix.x = NULL; + + int* ordering = cs_amd(1, &block_matrix); + vector<int> block_ordering(num_row_blocks, -1); + copy(ordering, ordering + num_row_blocks, &block_ordering[0]); + cs_free(ordering); + + vector<int> scalar_ordering; + BlockOrderingToScalarOrdering(row_blocks, block_ordering, &scalar_ordering); + + cs_dis* symbolic_factorization = + reinterpret_cast<cs_dis*>(cs_calloc(1, sizeof(cs_dis))); + symbolic_factorization->pinv = cs_pinv(&scalar_ordering[0], A->n); + cs* permuted_A = cs_symperm(A, symbolic_factorization->pinv, 0); + + symbolic_factorization->parent = cs_etree(permuted_A, 0); + int* postordering = cs_post(symbolic_factorization->parent, A->n); + int* column_counts = cs_counts(permuted_A, + symbolic_factorization->parent, + postordering, + 0); + cs_free(postordering); + cs_spfree(permuted_A); + + symbolic_factorization->cp = (int*) cs_malloc(A->n+1, sizeof(int)); + symbolic_factorization->lnz = cs_cumsum(symbolic_factorization->cp, + column_counts, + A->n); + symbolic_factorization->unz = symbolic_factorization->lnz; + + cs_free(column_counts); + + if (symbolic_factorization->lnz < 0) { + cs_sfree(symbolic_factorization); + symbolic_factorization = NULL; + } + + return symbolic_factorization; +} + cs_di CXSparse::CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A) { cs_di At; At.m = A->num_cols(); @@ -117,6 +186,20 @@ cs_di* CXSparse::CreateSparseMatrix(TripletSparseMatrix* tsm) { return cs_compress(&tsm_wrapper); } +void CXSparse::ApproximateMinimumDegreeOrdering(cs_di* A, int* ordering) { + int* cs_ordering = cs_amd(1, A); + copy(cs_ordering, cs_ordering + A->m, ordering); + cs_free(cs_ordering); +} + +cs_di* CXSparse::TransposeMatrix(cs_di* A) { + return cs_di_transpose(A, 1); +} + +cs_di* CXSparse::MatrixMatrixMultiply(cs_di* A, cs_di* B) { + return cs_di_multiply(A, B); +} + void CXSparse::Free(cs_di* sparse_matrix) { cs_di_spfree(sparse_matrix); } diff --git a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h index dd5eadc8da8..cd87908a43d 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h +++ b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h @@ -33,7 +33,9 @@ #ifndef CERES_NO_CXSPARSE +#include <vector> #include "cs.h" +#include "ceres/internal/port.h" namespace ceres { namespace internal { @@ -68,10 +70,49 @@ class CXSparse { // with Free. May return NULL if the compression or allocation fails. cs_di* CreateSparseMatrix(TripletSparseMatrix* A); + // B = A' + // + // The returned matrix should be deallocated with Free when not used + // anymore. + cs_di* TransposeMatrix(cs_di* A); + + // C = A * B + // + // The returned matrix should be deallocated with Free when not used + // anymore. + cs_di* MatrixMatrixMultiply(cs_di* A, cs_di* B); + // 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); + // Computes a symbolic factorization of A that can be used in + // SolveCholesky, but does not compute a fill-reducing ordering. + // + // The returned matrix should be deallocated with Free when not used anymore. + cs_dis* AnalyzeCholeskyWithNaturalOrdering(cs_di* A); + + // Computes a symbolic factorization of A that can be used in + // SolveCholesky. The difference from AnalyzeCholesky is that this + // function first detects the block sparsity of the matrix using + // information about the row and column blocks and uses this block + // sparse matrix to find a fill-reducing ordering. This ordering is + // then used to find a symbolic factorization. This can result in a + // significant performance improvement AnalyzeCholesky on block + // sparse matrices. + // + // The returned matrix should be deallocated with Free when not used + // anymore. + cs_dis* BlockAnalyzeCholesky(cs_di* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks); + + // Compute an fill-reducing approximate minimum degree ordering of + // the matrix A. ordering should be non-NULL and should point to + // enough memory to hold the ordering for the rows of A. + void ApproximateMinimumDegreeOrdering(cs_di* A, int* ordering); + void Free(cs_di* sparse_matrix); void Free(cs_dis* symbolic_factorization); @@ -84,6 +125,11 @@ class CXSparse { } // namespace internal } // namespace ceres +#else // CERES_NO_CXSPARSE + +class CXSparse {}; +typedef void cs_dis; + #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 index 96f55115a67..fbf3cbec9d2 100644 --- 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 @@ -33,9 +33,11 @@ #include <cstddef> #include "Eigen/Dense" +#include "ceres/blas.h" #include "ceres/dense_sparse_matrix.h" #include "ceres/internal/eigen.h" #include "ceres/internal/scoped_ptr.h" +#include "ceres/lapack.h" #include "ceres/linear_solver.h" #include "ceres/types.h" #include "ceres/wall_time.h" @@ -52,6 +54,18 @@ LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl( const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { + if (options_.dense_linear_algebra_library_type == EIGEN) { + return SolveUsingEigen(A, b, per_solve_options, x); + } else { + return SolveUsingLAPACK(A, b, per_solve_options, x); + } +} + +LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingEigen( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { EventLogger event_logger("DenseNormalCholeskySolver::Solve"); const int num_rows = A->num_rows(); @@ -62,6 +76,7 @@ LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl( lhs.setZero(); event_logger.AddEvent("Setup"); + // lhs += A'A // // Using rankUpdate instead of GEMM, exposes the fact that its the @@ -76,16 +91,66 @@ LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl( ConstVectorRef D(per_solve_options.D, num_cols); lhs += D.array().square().matrix().asDiagonal(); } + event_logger.AddEvent("Product"); LinearSolver::Summary summary; summary.num_iterations = 1; summary.termination_type = TOLERANCE; VectorRef(x, num_cols) = - lhs.selfadjointView<Eigen::Upper>().ldlt().solve(rhs); + lhs.selfadjointView<Eigen::Upper>().llt().solve(rhs); event_logger.AddEvent("Solve"); - return summary; } +LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingLAPACK( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + EventLogger event_logger("DenseNormalCholeskySolver::Solve"); + + if (per_solve_options.D != NULL) { + // Temporarily append a diagonal block to the A matrix, but undo + // it before returning the matrix to the user. + A->AppendDiagonal(per_solve_options.D); + } + + const int num_cols = A->num_cols(); + Matrix lhs(num_cols, num_cols); + event_logger.AddEvent("Setup"); + + // lhs = A'A + // + // Note: This is a bit delicate, it assumes that the stride on this + // matrix is the same as the number of rows. + BLAS::SymmetricRankKUpdate(A->num_rows(), + num_cols, + A->values(), + true, + 1.0, + 0.0, + lhs.data()); + + if (per_solve_options.D != NULL) { + // Undo the modifications to the matrix A. + A->RemoveDiagonal(); + } + + // TODO(sameeragarwal): Replace this with a gemv call for true blasness. + // rhs = A'b + VectorRef(x, num_cols) = + A->matrix().transpose() * ConstVectorRef(b, A->num_rows()); + event_logger.AddEvent("Product"); + + const int info = LAPACK::SolveInPlaceUsingCholesky(num_cols, lhs.data(), x); + event_logger.AddEvent("Solve"); + + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = info == 0 ? TOLERANCE : FAILURE; + + event_logger.AddEvent("TearDown"); + 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 index de47740583d..e35053f5234 100644 --- 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 @@ -85,6 +85,18 @@ class DenseNormalCholeskySolver: public DenseSparseMatrixSolver { const LinearSolver::PerSolveOptions& per_solve_options, double* x); + LinearSolver::Summary SolveUsingLAPACK( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + LinearSolver::Summary SolveUsingEigen( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + const LinearSolver::Options options_; CERES_DISALLOW_COPY_AND_ASSIGN(DenseNormalCholeskySolver); }; 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 1fb9709b42a..d76d58b51b5 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 @@ -30,12 +30,13 @@ #include "ceres/dense_qr_solver.h" -#include <cstddef> +#include <cstddef> #include "Eigen/Dense" #include "ceres/dense_sparse_matrix.h" #include "ceres/internal/eigen.h" #include "ceres/internal/scoped_ptr.h" +#include "ceres/lapack.h" #include "ceres/linear_solver.h" #include "ceres/types.h" #include "ceres/wall_time.h" @@ -44,13 +45,87 @@ namespace ceres { namespace internal { DenseQRSolver::DenseQRSolver(const LinearSolver::Options& options) - : options_(options) {} + : options_(options) { + work_.resize(1); +} LinearSolver::Summary DenseQRSolver::SolveImpl( DenseSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { + if (options_.dense_linear_algebra_library_type == EIGEN) { + return SolveUsingEigen(A, b, per_solve_options, x); + } else { + return SolveUsingLAPACK(A, b, per_solve_options, x); + } +} +LinearSolver::Summary DenseQRSolver::SolveUsingLAPACK( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + EventLogger event_logger("DenseQRSolver::Solve"); + + const int num_rows = A->num_rows(); + const int num_cols = A->num_cols(); + + if (per_solve_options.D != NULL) { + // Temporarily append a diagonal block to the A matrix, but undo + // it before returning the matrix to the user. + A->AppendDiagonal(per_solve_options.D); + } + + // TODO(sameeragarwal): Since we are copying anyways, the diagonal + // can be appended to the matrix instead of doing it on A. + lhs_ = A->matrix(); + + if (per_solve_options.D != NULL) { + // Undo the modifications to the matrix A. + A->RemoveDiagonal(); + } + + // rhs = [b;0] to account for the additional rows in the lhs. + if (rhs_.rows() != lhs_.rows()) { + rhs_.resize(lhs_.rows()); + } + rhs_.setZero(); + rhs_.head(num_rows) = ConstVectorRef(b, num_rows); + + if (work_.rows() == 1) { + const int work_size = + LAPACK::EstimateWorkSizeForQR(lhs_.rows(), lhs_.cols()); + VLOG(3) << "Working memory for Dense QR factorization: " + << work_size * sizeof(double); + work_.resize(work_size); + } + + const int info = LAPACK::SolveUsingQR(lhs_.rows(), + lhs_.cols(), + lhs_.data(), + work_.rows(), + work_.data(), + rhs_.data()); + event_logger.AddEvent("Solve"); + + LinearSolver::Summary summary; + summary.num_iterations = 1; + if (info == 0) { + VectorRef(x, num_cols) = rhs_.head(num_cols); + summary.termination_type = TOLERANCE; + } else { + summary.termination_type = FAILURE; + } + + event_logger.AddEvent("TearDown"); + return summary; +} + +LinearSolver::Summary DenseQRSolver::SolveUsingEigen( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { EventLogger event_logger("DenseQRSolver::Solve"); const int num_rows = A->num_rows(); @@ -73,7 +148,7 @@ LinearSolver::Summary DenseQRSolver::SolveImpl( event_logger.AddEvent("Setup"); // Solve the system. - VectorRef(x, num_cols) = A->matrix().colPivHouseholderQr().solve(rhs_); + VectorRef(x, num_cols) = A->matrix().householderQr().solve(rhs_); event_logger.AddEvent("Solve"); if (per_solve_options.D != NULL) { 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 f78fa72c5f3..e745c63cb44 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 @@ -90,8 +90,22 @@ class DenseQRSolver: public DenseSparseMatrixSolver { const LinearSolver::PerSolveOptions& per_solve_options, double* x); + LinearSolver::Summary SolveUsingEigen( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + LinearSolver::Summary SolveUsingLAPACK( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + const LinearSolver::Options options_; + ColMajorMatrix lhs_; Vector rhs_; + Vector work_; CERES_DISALLOW_COPY_AND_ASSIGN(DenseQRSolver); }; 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 9d58031ccfc..d67474fed32 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 @@ -31,10 +31,10 @@ #include "ceres/dense_sparse_matrix.h" #include <algorithm> -#include "ceres/matrix_proto.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -80,22 +80,6 @@ DenseSparseMatrix::DenseSparseMatrix(const ColMajorMatrix& m) has_diagonal_reserved_(false) { } -#ifndef CERES_NO_PROTOCOL_BUFFERS -DenseSparseMatrix::DenseSparseMatrix(const SparseMatrixProto& outer_proto) - : m_(Eigen::MatrixXd::Zero( - outer_proto.dense_matrix().num_rows(), - outer_proto.dense_matrix().num_cols())), - has_diagonal_appended_(false), - has_diagonal_reserved_(false) { - const DenseSparseMatrixProto& proto = outer_proto.dense_matrix(); - for (int i = 0; i < m_.rows(); ++i) { - for (int j = 0; j < m_.cols(); ++j) { - m_(i, j) = proto.values(m_.cols() * i + j); - } - } -} -#endif - void DenseSparseMatrix::SetZero() { m_.setZero(); } @@ -121,22 +105,6 @@ void DenseSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { *dense_matrix = m_.block(0, 0, num_rows(), num_cols()); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -void DenseSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { - CHECK(!has_diagonal_appended_) << "Not supported."; - outer_proto->Clear(); - DenseSparseMatrixProto* proto = outer_proto->mutable_dense_matrix(); - - proto->set_num_rows(num_rows()); - proto->set_num_cols(num_cols()); - - int num_nnz = num_nonzeros(); - for (int i = 0; i < num_nnz; ++i) { - proto->add_values(m_.data()[i]); - } -} -#endif - void DenseSparseMatrix::AppendDiagonal(double *d) { CHECK(!has_diagonal_appended_); if (!has_diagonal_reserved_) { 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 6c7b60ade13..981e2d14562 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 @@ -33,7 +33,6 @@ #ifndef CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ #define CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ -#include <glog/logging.h> #include "ceres/sparse_matrix.h" #include "ceres/internal/eigen.h" #include "ceres/internal/macros.h" @@ -43,7 +42,6 @@ namespace ceres { namespace internal { -class SparseMatrixProto; class TripletSparseMatrix; class DenseSparseMatrix : public SparseMatrix { @@ -52,9 +50,6 @@ class DenseSparseMatrix : public SparseMatrix { // m. This assumes that m does not have any repeated entries. explicit DenseSparseMatrix(const TripletSparseMatrix& m); explicit DenseSparseMatrix(const ColMajorMatrix& m); -#ifndef CERES_NO_PROTOCOL_BUFFERS - explicit DenseSparseMatrix(const SparseMatrixProto& proto); -#endif DenseSparseMatrix(int num_rows, int num_cols); DenseSparseMatrix(int num_rows, int num_cols, bool reserve_diagonal); @@ -68,9 +63,6 @@ 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_NO_PROTOCOL_BUFFERS - virtual void ToProto(SparseMatrixProto* proto) const; -#endif virtual void ToTextFile(FILE* file) const; virtual int num_rows() const; virtual int num_cols() const; diff --git a/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.cc b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.cc index a330ad2c7a2..c85c8e5cbf5 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.cc @@ -34,6 +34,7 @@ #include "Eigen/Dense" #include "ceres/array_utils.h" #include "ceres/internal/eigen.h" +#include "ceres/linear_least_squares_problems.h" #include "ceres/linear_solver.h" #include "ceres/polynomial.h" #include "ceres/sparse_matrix.h" @@ -52,8 +53,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), + min_diagonal_(options.min_lm_diagonal), + max_diagonal_(options.max_lm_diagonal), mu_(kMinMu), min_mu_(kMinMu), max_mu_(kMaxMu), @@ -127,7 +128,7 @@ TrustRegionStrategy::Summary DoglegStrategy::ComputeStep( ComputeCauchyPoint(jacobian); LinearSolver::Summary linear_solver_summary = - ComputeGaussNewtonStep(jacobian, residuals); + ComputeGaussNewtonStep(per_solve_options, jacobian, residuals); TrustRegionStrategy::Summary summary; summary.residual_norm = linear_solver_summary.residual_norm; @@ -507,6 +508,7 @@ bool DoglegStrategy::FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const { } LinearSolver::Summary DoglegStrategy::ComputeGaussNewtonStep( + const PerSolveOptions& per_solve_options, SparseMatrix* jacobian, const double* residuals) { const int n = jacobian->num_cols(); @@ -561,6 +563,22 @@ LinearSolver::Summary DoglegStrategy::ComputeGaussNewtonStep( solve_options, gauss_newton_step_.data()); + if (per_solve_options.dump_format_type == CONSOLE || + (per_solve_options.dump_format_type != CONSOLE && + !per_solve_options.dump_filename_base.empty())) { + if (!DumpLinearLeastSquaresProblem(per_solve_options.dump_filename_base, + per_solve_options.dump_format_type, + jacobian, + solve_options.D, + residuals, + gauss_newton_step_.data(), + 0)) { + LOG(ERROR) << "Unable to dump trust region problem." + << " Filename base: " + << per_solve_options.dump_filename_base; + } + } + if (linear_solver_summary.termination_type == FAILURE || !IsArrayValid(n, gauss_newton_step_.data())) { mu_ *= mu_increase_factor_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.h b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.h index 7131467d6ce..71c785cc3f7 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.h +++ b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.h @@ -79,8 +79,10 @@ class DoglegStrategy : public TrustRegionStrategy { 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); + LinearSolver::Summary ComputeGaussNewtonStep( + const PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals); void ComputeCauchyPoint(SparseMatrix* jacobian); void ComputeGradient(SparseMatrix* jacobian, const double* residuals); void ComputeTraditionalDoglegStep(double* step); diff --git a/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h b/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h index 2e6eec0e6d8..ca3a2fe1a88 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h +++ b/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h @@ -43,11 +43,12 @@ namespace ceres { namespace internal { -// Compare two vertices of a graph by their degrees. +// Compare two vertices of a graph by their degrees, if the degrees +// are equal then order them by their ids. template <typename Vertex> -class VertexDegreeLessThan { +class VertexTotalOrdering { public: - explicit VertexDegreeLessThan(const Graph<Vertex>& graph) + explicit VertexTotalOrdering(const Graph<Vertex>& graph) : graph_(graph) {} bool operator()(const Vertex& lhs, const Vertex& rhs) const { @@ -61,6 +62,20 @@ class VertexDegreeLessThan { const Graph<Vertex>& graph_; }; +template <typename Vertex> +class VertexDegreeLessThan { + public: + explicit VertexDegreeLessThan(const Graph<Vertex>& graph) + : graph_(graph) {} + + bool operator()(const Vertex& lhs, const Vertex& rhs) const { + return graph_.Neighbors(lhs).size() < graph_.Neighbors(rhs).size(); + } + + private: + const Graph<Vertex>& graph_; +}; + // Order the vertices of a graph using its (approximately) largest // independent set, where an independent set of a graph is a set of // vertices that have no edges connecting them. The maximum @@ -104,8 +119,83 @@ int IndependentSetOrdering(const Graph<Vertex>& graph, sort(vertex_queue.begin(), vertex_queue.end(), - VertexDegreeLessThan<Vertex>(graph)); + VertexTotalOrdering<Vertex>(graph)); + + // Iterate over vertex_queue. Pick the first white vertex, add it + // to the independent set. Mark it black and its neighbors grey. + for (int i = 0; i < vertex_queue.size(); ++i) { + const Vertex& vertex = vertex_queue[i]; + if (vertex_color[vertex] != kWhite) { + continue; + } + + ordering->push_back(vertex); + vertex_color[vertex] = kBlack; + const HashSet<Vertex>& neighbors = graph.Neighbors(vertex); + for (typename HashSet<Vertex>::const_iterator it = neighbors.begin(); + it != neighbors.end(); + ++it) { + vertex_color[*it] = kGrey; + } + } + + int independent_set_size = ordering->size(); + + // Iterate over the vertices and add all the grey vertices to the + // ordering. At this stage there should only be black or grey + // vertices in the graph. + for (typename vector<Vertex>::const_iterator it = vertex_queue.begin(); + it != vertex_queue.end(); + ++it) { + const Vertex vertex = *it; + DCHECK(vertex_color[vertex] != kWhite); + if (vertex_color[vertex] != kBlack) { + ordering->push_back(vertex); + } + } + + CHECK_EQ(ordering->size(), num_vertices); + return independent_set_size; +} + +// Same as above with one important difference. The ordering parameter +// is an input/output parameter which carries an initial ordering of +// the vertices of the graph. The greedy independent set algorithm +// starts by sorting the vertices in increasing order of their +// degree. The input ordering is used to stabilize this sort, i.e., if +// two vertices have the same degree then they are ordered in the same +// order in which they occur in "ordering". +// +// This is useful in eliminating non-determinism from the Schur +// ordering algorithm over all. +template <typename Vertex> +int StableIndependentSetOrdering(const Graph<Vertex>& graph, + vector<Vertex>* ordering) { + CHECK_NOTNULL(ordering); + const HashSet<Vertex>& vertices = graph.vertices(); + const int num_vertices = vertices.size(); + CHECK_EQ(vertices.size(), ordering->size()); + + // Colors for labeling the graph during the BFS. + const char kWhite = 0; + const char kGrey = 1; + const char kBlack = 2; + + vector<Vertex> vertex_queue(*ordering); + stable_sort(vertex_queue.begin(), vertex_queue.end(), + VertexDegreeLessThan<Vertex>(graph)); + + // Mark all vertices white. + HashMap<Vertex, char> vertex_color; + for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); + it != vertices.end(); + ++it) { + vertex_color[*it] = kWhite; + } + + ordering->clear(); + ordering->reserve(num_vertices); // Iterate over vertex_queue. Pick the first white vertex, add it // to the independent set. Mark it black and its neighbors grey. for (int i = 0; i < vertex_queue.size(); ++i) { 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 4af030a8535..32722bb6e8f 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 @@ -55,7 +55,7 @@ ImplicitSchurComplement::ImplicitSchurComplement(int num_eliminate_blocks, ImplicitSchurComplement::~ImplicitSchurComplement() { } -void ImplicitSchurComplement::Init(const BlockSparseMatrixBase& A, +void ImplicitSchurComplement::Init(const BlockSparseMatrix& A, const double* D, const double* b) { // Since initialization is reasonably heavy, perhaps we can save on @@ -161,7 +161,7 @@ void ImplicitSchurComplement::AddDiagonalAndInvert( m = m .selfadjointView<Eigen::Upper>() - .ldlt() + .llt() .solve(Matrix::Identity(row_block_size, row_block_size)); } } 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 b9ebaa4628e..c1bb6e19bab 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 @@ -44,7 +44,6 @@ namespace ceres { namespace internal { class BlockSparseMatrix; -class BlockSparseMatrixBase; // This class implements various linear algebraic operations related // to the Schur complement without explicitly forming it. @@ -110,7 +109,7 @@ class ImplicitSchurComplement : public LinearOperator { // is important that the matrix A have a BlockStructure object // associated with it and has a block structure that is compatible // with the SchurComplement solver. - void Init(const BlockSparseMatrixBase& A, const double* D, const double* b); + void Init(const BlockSparseMatrix& A, const double* D, const double* b); // y += Sx, where S is the Schur complement. virtual void RightMultiply(const double* x, double* y) const; diff --git a/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.cc b/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.cc new file mode 100644 index 00000000000..6ba38ec8eec --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.cc @@ -0,0 +1,239 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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/incomplete_lq_factorization.h" + +#include <vector> +#include <utility> +#include <cmath> +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Normalize a row and return it's norm. +inline double NormalizeRow(const int row, CompressedRowSparseMatrix* matrix) { + const int row_begin = matrix->rows()[row]; + const int row_end = matrix->rows()[row + 1]; + + double* values = matrix->mutable_values(); + double norm = 0.0; + for (int i = row_begin; i < row_end; ++i) { + norm += values[i] * values[i]; + } + + norm = sqrt(norm); + const double inverse_norm = 1.0 / norm; + for (int i = row_begin; i < row_end; ++i) { + values[i] *= inverse_norm; + } + + return norm; +} + +// Compute a(row_a,:) * b(row_b, :)' +inline double RowDotProduct(const CompressedRowSparseMatrix& a, + const int row_a, + const CompressedRowSparseMatrix& b, + const int row_b) { + const int* a_rows = a.rows(); + const int* a_cols = a.cols(); + const double* a_values = a.values(); + + const int* b_rows = b.rows(); + const int* b_cols = b.cols(); + const double* b_values = b.values(); + + const int row_a_end = a_rows[row_a + 1]; + const int row_b_end = b_rows[row_b + 1]; + + int idx_a = a_rows[row_a]; + int idx_b = b_rows[row_b]; + double dot_product = 0.0; + while (idx_a < row_a_end && idx_b < row_b_end) { + if (a_cols[idx_a] == b_cols[idx_b]) { + dot_product += a_values[idx_a++] * b_values[idx_b++]; + } + + while (a_cols[idx_a] < b_cols[idx_b] && idx_a < row_a_end) { + ++idx_a; + } + + while (a_cols[idx_a] > b_cols[idx_b] && idx_b < row_b_end) { + ++idx_b; + } + } + + return dot_product; +} + +struct SecondGreaterThan { + public: + bool operator()(const pair<int, double>& lhs, + const pair<int, double>& rhs) const { + return (fabs(lhs.second) > fabs(rhs.second)); + } +}; + +// In the row vector dense_row(0:num_cols), drop values smaller than +// the max_value * drop_tolerance. Of the remaining non-zero values, +// choose at most level_of_fill values and then add the resulting row +// vector to matrix. + +void DropEntriesAndAddRow(const Vector& dense_row, + const int num_entries, + const int level_of_fill, + const double drop_tolerance, + vector<pair<int, double> >* scratch, + CompressedRowSparseMatrix* matrix) { + int* rows = matrix->mutable_rows(); + int* cols = matrix->mutable_cols(); + double* values = matrix->mutable_values(); + int num_nonzeros = rows[matrix->num_rows()]; + + if (num_entries == 0) { + matrix->set_num_rows(matrix->num_rows() + 1); + rows[matrix->num_rows()] = num_nonzeros; + return; + } + + const double max_value = dense_row.head(num_entries).cwiseAbs().maxCoeff(); + const double threshold = drop_tolerance * max_value; + + int scratch_count = 0; + for (int i = 0; i < num_entries; ++i) { + if (fabs(dense_row[i]) > threshold) { + pair<int, double>& entry = (*scratch)[scratch_count]; + entry.first = i; + entry.second = dense_row[i]; + ++scratch_count; + } + } + + if (scratch_count > level_of_fill) { + nth_element(scratch->begin(), + scratch->begin() + level_of_fill, + scratch->begin() + scratch_count, + SecondGreaterThan()); + scratch_count = level_of_fill; + sort(scratch->begin(), scratch->begin() + scratch_count); + } + + for (int i = 0; i < scratch_count; ++i) { + const pair<int, double>& entry = (*scratch)[i]; + cols[num_nonzeros] = entry.first; + values[num_nonzeros] = entry.second; + ++num_nonzeros; + } + + matrix->set_num_rows(matrix->num_rows() + 1); + rows[matrix->num_rows()] = num_nonzeros; +} + +// Saad's Incomplete LQ factorization algorithm. +CompressedRowSparseMatrix* IncompleteLQFactorization( + const CompressedRowSparseMatrix& matrix, + const int l_level_of_fill, + const double l_drop_tolerance, + const int q_level_of_fill, + const double q_drop_tolerance) { + const int num_rows = matrix.num_rows(); + const int num_cols = matrix.num_cols(); + const int* rows = matrix.rows(); + const int* cols = matrix.cols(); + const double* values = matrix.values(); + + CompressedRowSparseMatrix* l = + new CompressedRowSparseMatrix(num_rows, + num_rows, + l_level_of_fill * num_rows); + l->set_num_rows(0); + + CompressedRowSparseMatrix q(num_rows, num_cols, q_level_of_fill * num_rows); + q.set_num_rows(0); + + int* l_rows = l->mutable_rows(); + int* l_cols = l->mutable_cols(); + double* l_values = l->mutable_values(); + + int* q_rows = q.mutable_rows(); + int* q_cols = q.mutable_cols(); + double* q_values = q.mutable_values(); + + Vector l_i(num_rows); + Vector q_i(num_cols); + vector<pair<int, double> > scratch(num_cols); + for (int i = 0; i < num_rows; ++i) { + // l_i = q * matrix(i,:)'); + l_i.setZero(); + for (int j = 0; j < i; ++j) { + l_i(j) = RowDotProduct(matrix, i, q, j); + } + DropEntriesAndAddRow(l_i, + i, + l_level_of_fill, + l_drop_tolerance, + &scratch, + l); + + // q_i = matrix(i,:) - q(0:i-1,:) * l_i); + q_i.setZero(); + for (int idx = rows[i]; idx < rows[i + 1]; ++idx) { + q_i(cols[idx]) = values[idx]; + } + + for (int j = l_rows[i]; j < l_rows[i + 1]; ++j) { + const int r = l_cols[j]; + const double lij = l_values[j]; + for (int idx = q_rows[r]; idx < q_rows[r + 1]; ++idx) { + q_i(q_cols[idx]) -= lij * q_values[idx]; + } + } + DropEntriesAndAddRow(q_i, + num_cols, + q_level_of_fill, + q_drop_tolerance, + &scratch, + &q); + + // lii = |qi| + l_cols[l->num_nonzeros()] = i; + l_values[l->num_nonzeros()] = NormalizeRow(i, &q); + l_rows[l->num_rows()] += 1; + } + + return l; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.h b/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.h new file mode 100644 index 00000000000..e678463cf8d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.h @@ -0,0 +1,90 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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_INCOMPLETE_LQ_FACTORIZATION_H_ +#define CERES_INTERNAL_INCOMPLETE_LQ_FACTORIZATION_H_ + +#include <vector> +#include <utility> +#include "ceres/compressed_row_sparse_matrix.h" + +namespace ceres { +namespace internal { + +// Incomplete LQ factorization as described in +// +// Preconditioning techniques for indefinite and nonsymmetric linear +// systems. Yousef Saad, Preprint RIACS-ILQ-TR, RIACS, NASA Ames +// Research Center, Moffett Field, CA, 1987. +// +// An incomplete LQ factorization of a matrix A is a decomposition +// +// A = LQ + E +// +// Where L is a lower triangular matrix, and Q is a near orthonormal +// matrix. The extent of orthonormality depends on E. E is the "drop" +// matrix. Each row of L has a maximum of l_level_of_fill entries, and +// all non-zero entries are within l_drop_tolerance of the largest +// entry. Each row of Q has a maximum of q_level_of_fill entries and +// all non-zero entries are within q_drop_tolerance of the largest +// entry. +// +// E is the error of the incomplete factorization. +// +// The purpose of incomplete factorizations is preconditioning and +// there one only needs the L matrix, therefore this function just +// returns L. +// +// Caller owns the result. +CompressedRowSparseMatrix* IncompleteLQFactorization( + const CompressedRowSparseMatrix& matrix, + const int l_level_of_fill, + const double l_drop_tolerance, + const int q_level_of_fill, + const double q_drop_tolerance); + +// In the row vector dense_row(0:num_cols), drop values smaller than +// the max_value * drop_tolerance. Of the remaining non-zero values, +// choose at most level_of_fill values and then add the resulting row +// vector to matrix. +// +// scratch is used to prevent allocations inside this function. It is +// assumed that scratch is of size matrix->num_cols(). +void DropEntriesAndAddRow(const Vector& dense_row, + const int num_entries, + const int level_of_fill, + const double drop_tolerance, + vector<pair<int, double> >* scratch, + CompressedRowSparseMatrix* matrix); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_INCOMPLETE_LQ_FACTORIZATION_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc index 15e0bdcd81a..1aac5657ce6 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 @@ -62,7 +62,7 @@ IterativeSchurComplementSolver::~IterativeSchurComplementSolver() { } LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { @@ -78,6 +78,17 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( } schur_complement_->Init(*A, per_solve_options.D, b); + const int num_schur_complement_blocks = + A->block_structure()->cols.size() - options_.elimination_groups[0]; + if (num_schur_complement_blocks == 0) { + VLOG(2) << "No parameter blocks left in the schur complement."; + LinearSolver::Summary cg_summary; + cg_summary.num_iterations = 0; + cg_summary.termination_type = TOLERANCE; + schur_complement_->BackSubstitute(NULL, x); + return cg_summary; + } + // Initialize the solution to the Schur complement system to zero. // // TODO(sameeragarwal): There maybe a better initialization than an @@ -97,8 +108,8 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( Preconditioner::Options preconditioner_options; preconditioner_options.type = options_.preconditioner_type; - preconditioner_options.sparse_linear_algebra_library = - options_.sparse_linear_algebra_library; + preconditioner_options.sparse_linear_algebra_library_type = + options_.sparse_linear_algebra_library_type; preconditioner_options.num_threads = options_.num_threads; preconditioner_options.row_block_size = options_.row_block_size; preconditioner_options.e_block_size = options_.e_block_size; @@ -116,16 +127,16 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( case SCHUR_JACOBI: if (preconditioner_.get() == NULL) { preconditioner_.reset( - new SchurJacobiPreconditioner( - *A->block_structure(), preconditioner_options)); + new SchurJacobiPreconditioner(*A->block_structure(), + preconditioner_options)); } break; case CLUSTER_JACOBI: case CLUSTER_TRIDIAGONAL: if (preconditioner_.get() == NULL) { preconditioner_.reset( - new VisibilityBasedPreconditioner( - *A->block_structure(), preconditioner_options)); + new VisibilityBasedPreconditioner(*A->block_structure(), + preconditioner_options)); } break; default: diff --git a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h index f8abe04c142..b056a694478 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h @@ -39,7 +39,7 @@ namespace ceres { namespace internal { -class BlockSparseMatrixBase; +class BlockSparseMatrix; class ImplicitSchurComplement; class Preconditioner; @@ -67,14 +67,14 @@ class Preconditioner; // a proof of this fact and others related to this solver please see // the section on Domain Decomposition Methods in Saad's book // "Iterative Methods for Sparse Linear Systems". -class IterativeSchurComplementSolver : public BlockSparseMatrixBaseSolver { +class IterativeSchurComplementSolver : public BlockSparseMatrixSolver { public: explicit IterativeSchurComplementSolver(const LinearSolver::Options& options); virtual ~IterativeSchurComplementSolver(); private: virtual LinearSolver::Summary SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& options, double* x); diff --git a/extern/libmv/third_party/ceres/internal/ceres/lapack.cc b/extern/libmv/third_party/ceres/internal/ceres/lapack.cc new file mode 100644 index 00000000000..73bfa69cbbd --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/lapack.cc @@ -0,0 +1,157 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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/lapack.h" +#include "glog/logging.h" + +// C interface to the LAPACK Cholesky factorization and triangular solve. +extern "C" void dpotrf_(char* uplo, + int* n, + double* a, + int* lda, + int* info); + +extern "C" void dpotrs_(char* uplo, + int* n, + int* nrhs, + double* a, + int* lda, + double* b, + int* ldb, + int* info); + +extern "C" void dgels_(char* uplo, + int* m, + int* n, + int* nrhs, + double* a, + int* lda, + double* b, + int* ldb, + double* work, + int* lwork, + int* info); + + +namespace ceres { +namespace internal { + +int LAPACK::SolveInPlaceUsingCholesky(int num_rows, + const double* in_lhs, + double* rhs_and_solution) { +#ifdef CERES_NO_LAPACK + LOG(FATAL) << "Ceres was built without a BLAS library."; + return -1; +#else + char uplo = 'L'; + int n = num_rows; + int info = 0; + int nrhs = 1; + double* lhs = const_cast<double*>(in_lhs); + + dpotrf_(&uplo, &n, lhs, &n, &info); + if (info != 0) { + LOG(INFO) << "Cholesky factorization (dpotrf) failed: " << info; + return info; + } + + dpotrs_(&uplo, &n, &nrhs, lhs, &n, rhs_and_solution, &n, &info); + if (info != 0) { + LOG(INFO) << "Triangular solve (dpotrs) failed: " << info; + } + + return info; +#endif +}; + +int LAPACK::EstimateWorkSizeForQR(int num_rows, int num_cols) { +#ifdef CERES_NO_LAPACK + LOG(FATAL) << "Ceres was built without a LAPACK library."; + return -1; +#else + char trans = 'N'; + int nrhs = 1; + int lwork = -1; + double work; + int info = 0; + dgels_(&trans, + &num_rows, + &num_cols, + &nrhs, + NULL, + &num_rows, + NULL, + &num_rows, + &work, + &lwork, + &info); + + CHECK_EQ(info, 0); + return work; +#endif +} + +int LAPACK::SolveUsingQR(int num_rows, + int num_cols, + const double* in_lhs, + int work_size, + double* work, + double* rhs_and_solution) { +#ifdef CERES_NO_LAPACK + LOG(FATAL) << "Ceres was built without a LAPACK library."; + return -1; +#else + char trans = 'N'; + int m = num_rows; + int n = num_cols; + int nrhs = 1; + int lda = num_rows; + int ldb = num_rows; + int info = 0; + double* lhs = const_cast<double*>(in_lhs); + + dgels_(&trans, + &m, + &n, + &nrhs, + lhs, + &lda, + rhs_and_solution, + &ldb, + work, + &work_size, + &info); + + return info; +#endif +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/lapack.h b/extern/libmv/third_party/ceres/internal/ceres/lapack.h new file mode 100644 index 00000000000..4f3a88c700a --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/lapack.h @@ -0,0 +1,88 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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_LAPACK_H_ +#define CERES_INTERNAL_LAPACK_H_ + +namespace ceres { +namespace internal { + +class LAPACK { + public: + // Solve + // + // lhs * solution = rhs + // + // using a Cholesky factorization. Here + // lhs is a symmetric positive definite matrix. It is assumed to be + // column major and only the lower triangular part of the matrix is + // referenced. + // + // This function uses the LAPACK dpotrf and dpotrs routines. + // + // The return value is zero if the solve is successful. + static int SolveInPlaceUsingCholesky(int num_rows, + const double* lhs, + double* rhs_and_solution); + + // The SolveUsingQR function requires a buffer for its temporary + // computation. This function given the size of the lhs matrix will + // return the size of the buffer needed. + static int EstimateWorkSizeForQR(int num_rows, int num_cols); + + // Solve + // + // lhs * solution = rhs + // + // using a dense QR factorization. lhs is an arbitrary (possibly + // rectangular) matrix with full column rank. + // + // work is an array of size work_size that this routine uses for its + // temporary storage. The optimal size of this array can be obtained + // by calling EstimateWorkSizeForQR. + // + // When calling, rhs_and_solution contains the rhs, and upon return + // the first num_col entries are the solution. + // + // This function uses the LAPACK dgels routine. + // + // The return value is zero if the solve is successful. + static int SolveUsingQR(int num_rows, + int num_cols, + const double* lhs, + int work_size, + double* work, + double* rhs_and_solution); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LAPACK_H_ 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 index 9e6a59e3813..fad7c1f3258 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt_strategy.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt_strategy.cc @@ -34,6 +34,7 @@ #include "Eigen/Core" #include "ceres/array_utils.h" #include "ceres/internal/eigen.h" +#include "ceres/linear_least_squares_problems.h" #include "ceres/linear_solver.h" #include "ceres/sparse_matrix.h" #include "ceres/trust_region_strategy.h" @@ -48,8 +49,8 @@ LevenbergMarquardtStrategy::LevenbergMarquardtStrategy( : 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), + min_diagonal_(options.min_lm_diagonal), + max_diagonal_(options.max_lm_diagonal), decrease_factor_(2.0), reuse_diagonal_(false) { CHECK_NOTNULL(linear_solver_); @@ -111,9 +112,24 @@ TrustRegionStrategy::Summary LevenbergMarquardtStrategy::ComputeStep( } else { VectorRef(step, num_parameters) *= -1.0; } - reuse_diagonal_ = true; + if (per_solve_options.dump_format_type == CONSOLE || + (per_solve_options.dump_format_type != CONSOLE && + !per_solve_options.dump_filename_base.empty())) { + if (!DumpLinearLeastSquaresProblem(per_solve_options.dump_filename_base, + per_solve_options.dump_format_type, + jacobian, + solve_options.D, + residuals, + step, + 0)) { + LOG(ERROR) << "Unable to dump trust region problem." + << " Filename base: " << per_solve_options.dump_filename_base; + } + } + + TrustRegionStrategy::Summary summary; summary.residual_norm = linear_solver_summary.residual_norm; summary.num_iterations = linear_solver_summary.num_iterations; diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search.cc b/extern/libmv/third_party/ceres/internal/ceres/line_search.cc index 437f742607f..8323896915a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/line_search.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search.cc @@ -31,12 +31,12 @@ #ifndef CERES_NO_LINE_SEARCH_MINIMIZER #include "ceres/line_search.h" -#include <glog/logging.h> #include "ceres/fpclassify.h" #include "ceres/evaluator.h" #include "ceres/internal/eigen.h" #include "ceres/polynomial.h" - +#include "ceres/stringprintf.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -64,6 +64,39 @@ FunctionSample ValueAndGradientSample(const double x, } // namespace +// Convenience stream operator for pushing FunctionSamples into log messages. +std::ostream& operator<<(std::ostream &os, + const FunctionSample& sample) { + os << "[x: " << sample.x << ", value: " << sample.value + << ", gradient: " << sample.gradient << ", value_is_valid: " + << std::boolalpha << sample.value_is_valid << ", gradient_is_valid: " + << std::boolalpha << sample.gradient_is_valid << "]"; + return os; +} + +LineSearch::LineSearch(const LineSearch::Options& options) + : options_(options) {} + +LineSearch* LineSearch::Create(const LineSearchType line_search_type, + const LineSearch::Options& options, + string* error) { + LineSearch* line_search = NULL; + switch (line_search_type) { + case ceres::ARMIJO: + line_search = new ArmijoLineSearch(options); + break; + case ceres::WOLFE: + line_search = new WolfeLineSearch(options); + break; + default: + *error = string("Invalid line search algorithm type: ") + + LineSearchTypeToString(line_search_type) + + string(", unable to create line search."); + return NULL; + } + return line_search; +} + LineSearchFunction::LineSearchFunction(Evaluator* evaluator) : evaluator_(evaluator), position_(evaluator->NumParameters()), @@ -79,7 +112,7 @@ void LineSearchFunction::Init(const Vector& position, direction_ = direction; } -bool LineSearchFunction::Evaluate(const double x, double* f, double* g) { +bool LineSearchFunction::Evaluate(double x, double* f, double* g) { scaled_direction_ = x * direction_; if (!evaluator_->Plus(position_.data(), scaled_direction_.data(), @@ -104,110 +137,608 @@ bool LineSearchFunction::Evaluate(const double x, double* f, double* g) { return IsFinite(*f) && IsFinite(*g); } -void ArmijoLineSearch::Search(const LineSearch::Options& options, - const double initial_step_size, +double LineSearchFunction::DirectionInfinityNorm() const { + return direction_.lpNorm<Eigen::Infinity>(); +} + +// Returns step_size \in [min_step_size, max_step_size] which minimizes the +// polynomial of degree defined by interpolation_type which interpolates all +// of the provided samples with valid values. +double LineSearch::InterpolatingPolynomialMinimizingStepSize( + const LineSearchInterpolationType& interpolation_type, + const FunctionSample& lowerbound, + const FunctionSample& previous, + const FunctionSample& current, + const double min_step_size, + const double max_step_size) const { + if (!current.value_is_valid || + (interpolation_type == BISECTION && + max_step_size <= current.x)) { + // Either: sample is invalid; or we are using BISECTION and contracting + // the step size. + return min(max(current.x * 0.5, min_step_size), max_step_size); + } else if (interpolation_type == BISECTION) { + CHECK_GT(max_step_size, current.x); + // We are expanding the search (during a Wolfe bracketing phase) using + // BISECTION interpolation. Using BISECTION when trying to expand is + // strictly speaking an oxymoron, but we define this to mean always taking + // the maximum step size so that the Armijo & Wolfe implementations are + // agnostic to the interpolation type. + return max_step_size; + } + // Only check if lower-bound is valid here, where it is required + // to avoid replicating current.value_is_valid == false + // behaviour in WolfeLineSearch. + CHECK(lowerbound.value_is_valid) + << "Ceres bug: lower-bound sample for interpolation is invalid, " + << "please contact the developers!, interpolation_type: " + << LineSearchInterpolationTypeToString(interpolation_type) + << ", lowerbound: " << lowerbound << ", previous: " << previous + << ", current: " << current; + + // Select step size by interpolating the function and gradient values + // and minimizing the corresponding polynomial. + vector<FunctionSample> samples; + samples.push_back(lowerbound); + + if (interpolation_type == QUADRATIC) { + // Two point interpolation using function values and the + // gradient at the lower bound. + samples.push_back(ValueSample(current.x, current.value)); + + if (previous.value_is_valid) { + // Three point interpolation, using function values and the + // gradient at the lower bound. + samples.push_back(ValueSample(previous.x, previous.value)); + } + } else if (interpolation_type == CUBIC) { + // Two point interpolation using the function values and the gradients. + samples.push_back(current); + + if (previous.value_is_valid) { + // Three point interpolation using the function values and + // the gradients. + samples.push_back(previous); + } + } else { + LOG(FATAL) << "Ceres bug: No handler for interpolation_type: " + << LineSearchInterpolationTypeToString(interpolation_type) + << ", please contact the developers!"; + } + + double step_size = 0.0, unused_min_value = 0.0; + MinimizeInterpolatingPolynomial(samples, min_step_size, max_step_size, + &step_size, &unused_min_value); + return step_size; +} + +ArmijoLineSearch::ArmijoLineSearch(const LineSearch::Options& options) + : LineSearch(options) {} + +void ArmijoLineSearch::Search(const double step_size_estimate, const double initial_cost, const double initial_gradient, Summary* summary) { *CHECK_NOTNULL(summary) = LineSearch::Summary(); - Function* function = options.function; - - double previous_step_size = 0.0; - double previous_cost = 0.0; - double previous_gradient = 0.0; - bool previous_step_size_is_valid = false; - - double step_size = initial_step_size; - double cost = 0.0; - double gradient = 0.0; - bool step_size_is_valid = false; - - ++summary->num_evaluations; - step_size_is_valid = - function->Evaluate(step_size, - &cost, - options.interpolation_degree < 2 ? NULL : &gradient); - while (!step_size_is_valid || cost > (initial_cost - + options.sufficient_decrease - * initial_gradient - * step_size)) { - // If step_size_is_valid is not true we treat it as if the cost at - // that point is not large enough to satisfy the sufficient - // decrease condition. - - const double current_step_size = step_size; - // Backtracking search. Each iteration of this loop finds a new point - - if ((options.interpolation_degree == 0) || !step_size_is_valid) { - // Backtrack by halving the step_size; - step_size *= 0.5; - } else { - // Backtrack by interpolating the function and gradient values - // and minimizing the corresponding polynomial. - - vector<FunctionSample> samples; - samples.push_back(ValueAndGradientSample(0.0, - initial_cost, - initial_gradient)); - - if (options.interpolation_degree == 1) { - // Two point interpolation using function values and the - // initial gradient. - samples.push_back(ValueSample(step_size, cost)); - - if (options.use_higher_degree_interpolation_when_possible && - summary->num_evaluations > 1 && - previous_step_size_is_valid) { - // Three point interpolation, using function values and the - // initial gradient. - samples.push_back(ValueSample(previous_step_size, previous_cost)); - } - } else { - // Two point interpolation using the function values and the gradients. - samples.push_back(ValueAndGradientSample(step_size, - cost, - gradient)); - - if (options.use_higher_degree_interpolation_when_possible && - summary->num_evaluations > 1 && - previous_step_size_is_valid) { - // Three point interpolation using the function values and - // the gradients. - samples.push_back(ValueAndGradientSample(previous_step_size, - previous_cost, - previous_gradient)); - } - } + CHECK_GE(step_size_estimate, 0.0); + CHECK_GT(options().sufficient_decrease, 0.0); + CHECK_LT(options().sufficient_decrease, 1.0); + CHECK_GT(options().max_num_iterations, 0); + Function* function = options().function; + + // Note initial_cost & initial_gradient are evaluated at step_size = 0, + // not step_size_estimate, which is our starting guess. + const FunctionSample initial_position = + ValueAndGradientSample(0.0, initial_cost, initial_gradient); + + FunctionSample previous = ValueAndGradientSample(0.0, 0.0, 0.0); + previous.value_is_valid = false; + + FunctionSample current = ValueAndGradientSample(step_size_estimate, 0.0, 0.0); + current.value_is_valid = false; - double min_value; - MinimizeInterpolatingPolynomial(samples, 0.0, current_step_size, - &step_size, &min_value); - step_size = - min(max(step_size, - options.min_relative_step_size_change * current_step_size), - options.max_relative_step_size_change * current_step_size); + const bool interpolation_uses_gradients = + options().interpolation_type == CUBIC; + const double descent_direction_max_norm = + static_cast<const LineSearchFunction*>(function)->DirectionInfinityNorm(); + + ++summary->num_function_evaluations; + if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; } + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + interpolation_uses_gradients + ? ¤t.gradient : NULL); + current.gradient_is_valid = + interpolation_uses_gradients && current.value_is_valid; + while (!current.value_is_valid || + current.value > (initial_cost + + options().sufficient_decrease + * initial_gradient + * current.x)) { + // If current.value_is_valid is false, we treat it as if the cost at that + // point is not large enough to satisfy the sufficient decrease condition. + ++summary->num_iterations; + if (summary->num_iterations >= options().max_num_iterations) { + summary->error = + StringPrintf("Line search failed: Armijo failed to find a point " + "satisfying the sufficient decrease condition within " + "specified max_num_iterations: %d.", + options().max_num_iterations); + LOG(WARNING) << summary->error; + return; } - previous_step_size = current_step_size; - previous_cost = cost; - previous_gradient = gradient; + const double step_size = + this->InterpolatingPolynomialMinimizingStepSize( + options().interpolation_type, + initial_position, + previous, + current, + (options().max_step_contraction * current.x), + (options().min_step_contraction * current.x)); - if (fabs(initial_gradient) * step_size < options.step_size_threshold) { - LOG(WARNING) << "Line search failed: step_size too small: " << step_size; + if (step_size * descent_direction_max_norm < options().min_step_size) { + summary->error = + StringPrintf("Line search failed: step_size too small: %.5e " + "with descent_direction_max_norm: %.5e.", step_size, + descent_direction_max_norm); + LOG(WARNING) << summary->error; return; } - ++summary->num_evaluations; - step_size_is_valid = - function->Evaluate(step_size, - &cost, - options.interpolation_degree < 2 ? NULL : &gradient); + previous = current; + current.x = step_size; + + ++summary->num_function_evaluations; + if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; } + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + interpolation_uses_gradients + ? ¤t.gradient : NULL); + current.gradient_is_valid = + interpolation_uses_gradients && current.value_is_valid; + } + + summary->optimal_step_size = current.x; + summary->success = true; +} + +WolfeLineSearch::WolfeLineSearch(const LineSearch::Options& options) + : LineSearch(options) {} + +void WolfeLineSearch::Search(const double step_size_estimate, + const double initial_cost, + const double initial_gradient, + Summary* summary) { + *CHECK_NOTNULL(summary) = LineSearch::Summary(); + // All parameters should have been validated by the Solver, but as + // invalid values would produce crazy nonsense, hard check them here. + CHECK_GE(step_size_estimate, 0.0); + CHECK_GT(options().sufficient_decrease, 0.0); + CHECK_GT(options().sufficient_curvature_decrease, + options().sufficient_decrease); + CHECK_LT(options().sufficient_curvature_decrease, 1.0); + CHECK_GT(options().max_step_expansion, 1.0); + + // Note initial_cost & initial_gradient are evaluated at step_size = 0, + // not step_size_estimate, which is our starting guess. + const FunctionSample initial_position = + ValueAndGradientSample(0.0, initial_cost, initial_gradient); + + bool do_zoom_search = false; + // Important: The high/low in bracket_high & bracket_low refer to their + // _function_ values, not their step sizes i.e. it is _not_ required that + // bracket_low.x < bracket_high.x. + FunctionSample solution, bracket_low, bracket_high; + + // Wolfe bracketing phase: Increases step_size until either it finds a point + // that satisfies the (strong) Wolfe conditions, or an interval that brackets + // step sizes which satisfy the conditions. From Nocedal & Wright [1] p61 the + // interval: (step_size_{k-1}, step_size_{k}) contains step lengths satisfying + // the strong Wolfe conditions if one of the following conditions are met: + // + // 1. step_size_{k} violates the sufficient decrease (Armijo) condition. + // 2. f(step_size_{k}) >= f(step_size_{k-1}). + // 3. f'(step_size_{k}) >= 0. + // + // Caveat: If f(step_size_{k}) is invalid, then step_size is reduced, ignoring + // this special case, step_size monotonically increases during bracketing. + if (!this->BracketingPhase(initial_position, + step_size_estimate, + &bracket_low, + &bracket_high, + &do_zoom_search, + summary) && + summary->num_iterations < options().max_num_iterations) { + // Failed to find either a valid point or a valid bracket, but we did not + // run out of iterations. + return; + } + if (!do_zoom_search) { + // Either: Bracketing phase already found a point satisfying the strong + // Wolfe conditions, thus no Zoom required. + // + // Or: Bracketing failed to find a valid bracket or a point satisfying the + // strong Wolfe conditions within max_num_iterations. As this is an + // 'artificial' constraint, and we would otherwise fail to produce a valid + // point when ArmijoLineSearch would succeed, we return the lowest point + // found thus far which satsifies the Armijo condition (but not the Wolfe + // conditions). + CHECK(bracket_low.value_is_valid) + << "Ceres bug: Bracketing produced an invalid bracket_low, please " + << "contact the developers!, bracket_low: " << bracket_low + << ", bracket_high: " << bracket_high << ", num_iterations: " + << summary->num_iterations << ", max_num_iterations: " + << options().max_num_iterations; + summary->optimal_step_size = bracket_low.x; + summary->success = true; + return; + } + + // Wolfe Zoom phase: Called when the Bracketing phase finds an interval of + // non-zero, finite width that should bracket step sizes which satisfy the + // (strong) Wolfe conditions (before finding a step size that satisfies the + // conditions). Zoom successively decreases the size of the interval until a + // step size which satisfies the Wolfe conditions is found. The interval is + // defined by bracket_low & bracket_high, which satisfy: + // + // 1. The interval bounded by step sizes: bracket_low.x & bracket_high.x + // contains step sizes that satsify the strong Wolfe conditions. + // 2. bracket_low.x is of all the step sizes evaluated *which satisifed the + // Armijo sufficient decrease condition*, the one which generated the + // smallest function value, i.e. bracket_low.value < + // f(all other steps satisfying Armijo). + // - Note that this does _not_ (necessarily) mean that initially + // bracket_low.value < bracket_high.value (although this is typical) + // e.g. when bracket_low = initial_position, and bracket_high is the + // first sample, and which does not satisfy the Armijo condition, + // but still has bracket_high.value < initial_position.value. + // 3. bracket_high is chosen after bracket_low, s.t. + // bracket_low.gradient * (bracket_high.x - bracket_low.x) < 0. + if (!this->ZoomPhase(initial_position, + bracket_low, + bracket_high, + &solution, + summary) && !solution.value_is_valid) { + // Failed to find a valid point (given the specified decrease parameters) + // within the specified bracket. + return; } + // Ensure that if we ran out of iterations whilst zooming the bracket, or + // shrank the bracket width to < tolerance and failed to find a point which + // satisfies the strong Wolfe curvature condition, that we return the point + // amongst those found thus far, which minimizes f() and satisfies the Armijo + // condition. + solution = + solution.value_is_valid && solution.value <= bracket_low.value + ? solution : bracket_low; - summary->optimal_step_size = step_size; + summary->optimal_step_size = solution.x; summary->success = true; } +// Returns true iff bracket_low & bracket_high bound a bracket that contains +// points which satisfy the strong Wolfe conditions. Otherwise, on return false, +// if we stopped searching due to the 'artificial' condition of reaching +// max_num_iterations, bracket_low is the step size amongst all those +// tested, which satisfied the Armijo decrease condition and minimized f(). +bool WolfeLineSearch::BracketingPhase( + const FunctionSample& initial_position, + const double step_size_estimate, + FunctionSample* bracket_low, + FunctionSample* bracket_high, + bool* do_zoom_search, + Summary* summary) { + Function* function = options().function; + + FunctionSample previous = initial_position; + FunctionSample current = ValueAndGradientSample(step_size_estimate, 0.0, 0.0); + current.value_is_valid = false; + + const bool interpolation_uses_gradients = + options().interpolation_type == CUBIC; + const double descent_direction_max_norm = + static_cast<const LineSearchFunction*>(function)->DirectionInfinityNorm(); + + *do_zoom_search = false; + *bracket_low = initial_position; + + ++summary->num_function_evaluations; + if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; } + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + interpolation_uses_gradients + ? ¤t.gradient : NULL); + current.gradient_is_valid = + interpolation_uses_gradients && current.value_is_valid; + + while (true) { + ++summary->num_iterations; + + if (current.value_is_valid && + (current.value > (initial_position.value + + options().sufficient_decrease + * initial_position.gradient + * current.x) || + (previous.value_is_valid && current.value > previous.value))) { + // Bracket found: current step size violates Armijo sufficient decrease + // condition, or has stepped past an inflection point of f() relative to + // previous step size. + *do_zoom_search = true; + *bracket_low = previous; + *bracket_high = current; + break; + } + + // Irrespective of the interpolation type we are using, we now need the + // gradient at the current point (which satisfies the Armijo condition) + // in order to check the strong Wolfe conditions. + if (!interpolation_uses_gradients) { + ++summary->num_function_evaluations; + ++summary->num_gradient_evaluations; + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + ¤t.gradient); + current.gradient_is_valid = current.value_is_valid; + } + + if (current.value_is_valid && + fabs(current.gradient) <= + -options().sufficient_curvature_decrease * initial_position.gradient) { + // Current step size satisfies the strong Wolfe conditions, and is thus a + // valid termination point, therefore a Zoom not required. + *bracket_low = current; + *bracket_high = current; + break; + + } else if (current.value_is_valid && current.gradient >= 0) { + // Bracket found: current step size has stepped past an inflection point + // of f(), but Armijo sufficient decrease is still satisfied and + // f(current) is our best minimum thus far. Remember step size + // monotonically increases, thus previous_step_size < current_step_size + // even though f(previous) > f(current). + *do_zoom_search = true; + // Note inverse ordering from first bracket case. + *bracket_low = current; + *bracket_high = previous; + break; + + } else if (summary->num_iterations >= options().max_num_iterations) { + // Check num iterations bound here so that we always evaluate the + // max_num_iterations-th iteration against all conditions, and + // then perform no additional (unused) evaluations. + summary->error = + StringPrintf("Line search failed: Wolfe bracketing phase failed to " + "find a point satisfying strong Wolfe conditions, or a " + "bracket containing such a point within specified " + "max_num_iterations: %d", options().max_num_iterations); + LOG(WARNING) << summary->error; + // Ensure that bracket_low is always set to the step size amongst all + // those tested which minimizes f() and satisfies the Armijo condition + // when we terminate due to the 'artificial' max_num_iterations condition. + *bracket_low = + current.value_is_valid && current.value < bracket_low->value + ? current : *bracket_low; + return false; + } + // Either: f(current) is invalid; or, f(current) is valid, but does not + // satisfy the strong Wolfe conditions itself, or the conditions for + // being a boundary of a bracket. + + // If f(current) is valid, (but meets no criteria) expand the search by + // increasing the step size. + const double max_step_size = + current.value_is_valid + ? (current.x * options().max_step_expansion) : current.x; + + // We are performing 2-point interpolation only here, but the API of + // InterpolatingPolynomialMinimizingStepSize() allows for up to + // 3-point interpolation, so pad call with a sample with an invalid + // value that will therefore be ignored. + const FunctionSample unused_previous; + DCHECK(!unused_previous.value_is_valid); + // Contracts step size if f(current) is not valid. + const double step_size = + this->InterpolatingPolynomialMinimizingStepSize( + options().interpolation_type, + previous, + unused_previous, + current, + previous.x, + max_step_size); + if (step_size * descent_direction_max_norm < options().min_step_size) { + summary->error = + StringPrintf("Line search failed: step_size too small: %.5e " + "with descent_direction_max_norm: %.5e", step_size, + descent_direction_max_norm); + LOG(WARNING) << summary->error; + return false; + } + + previous = current.value_is_valid ? current : previous; + current.x = step_size; + + ++summary->num_function_evaluations; + if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; } + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + interpolation_uses_gradients + ? ¤t.gradient : NULL); + current.gradient_is_valid = + interpolation_uses_gradients && current.value_is_valid; + } + // Either we have a valid point, defined as a bracket of zero width, in which + // case no zoom is required, or a valid bracket in which to zoom. + return true; +} + +// Returns true iff solution satisfies the strong Wolfe conditions. Otherwise, +// on return false, if we stopped searching due to the 'artificial' condition of +// reaching max_num_iterations, solution is the step size amongst all those +// tested, which satisfied the Armijo decrease condition and minimized f(). +bool WolfeLineSearch::ZoomPhase(const FunctionSample& initial_position, + FunctionSample bracket_low, + FunctionSample bracket_high, + FunctionSample* solution, + Summary* summary) { + Function* function = options().function; + + CHECK(bracket_low.value_is_valid && bracket_low.gradient_is_valid) + << "Ceres bug: f_low input to Wolfe Zoom invalid, please contact " + << "the developers!, initial_position: " << initial_position + << ", bracket_low: " << bracket_low + << ", bracket_high: "<< bracket_high; + // We do not require bracket_high.gradient_is_valid as the gradient condition + // for a valid bracket is only dependent upon bracket_low.gradient, and + // in order to minimize jacobian evaluations, bracket_high.gradient may + // not have been calculated (if bracket_high.value does not satisfy the + // Armijo sufficient decrease condition and interpolation method does not + // require it). + CHECK(bracket_high.value_is_valid) + << "Ceres bug: f_high input to Wolfe Zoom invalid, please " + << "contact the developers!, initial_position: " << initial_position + << ", bracket_low: " << bracket_low + << ", bracket_high: "<< bracket_high; + CHECK_LT(bracket_low.gradient * + (bracket_high.x - bracket_low.x), 0.0) + << "Ceres bug: f_high input to Wolfe Zoom does not satisfy gradient " + << "condition combined with f_low, please contact the developers!" + << ", initial_position: " << initial_position + << ", bracket_low: " << bracket_low + << ", bracket_high: "<< bracket_high; + + const int num_bracketing_iterations = summary->num_iterations; + const bool interpolation_uses_gradients = + options().interpolation_type == CUBIC; + const double descent_direction_max_norm = + static_cast<const LineSearchFunction*>(function)->DirectionInfinityNorm(); + + while (true) { + // Set solution to bracket_low, as it is our best step size (smallest f()) + // found thus far and satisfies the Armijo condition, even though it does + // not satisfy the Wolfe condition. + *solution = bracket_low; + if (summary->num_iterations >= options().max_num_iterations) { + summary->error = + StringPrintf("Line search failed: Wolfe zoom phase failed to " + "find a point satisfying strong Wolfe conditions " + "within specified max_num_iterations: %d, " + "(num iterations taken for bracketing: %d).", + options().max_num_iterations, num_bracketing_iterations); + LOG(WARNING) << summary->error; + return false; + } + if (fabs(bracket_high.x - bracket_low.x) * descent_direction_max_norm + < options().min_step_size) { + // Bracket width has been reduced below tolerance, and no point satisfying + // the strong Wolfe conditions has been found. + summary->error = + StringPrintf("Line search failed: Wolfe zoom bracket width: %.5e " + "too small with descent_direction_max_norm: %.5e.", + fabs(bracket_high.x - bracket_low.x), + descent_direction_max_norm); + LOG(WARNING) << summary->error; + return false; + } + + ++summary->num_iterations; + // Polynomial interpolation requires inputs ordered according to step size, + // not f(step size). + const FunctionSample& lower_bound_step = + bracket_low.x < bracket_high.x ? bracket_low : bracket_high; + const FunctionSample& upper_bound_step = + bracket_low.x < bracket_high.x ? bracket_high : bracket_low; + // We are performing 2-point interpolation only here, but the API of + // InterpolatingPolynomialMinimizingStepSize() allows for up to + // 3-point interpolation, so pad call with a sample with an invalid + // value that will therefore be ignored. + const FunctionSample unused_previous; + DCHECK(!unused_previous.value_is_valid); + solution->x = + this->InterpolatingPolynomialMinimizingStepSize( + options().interpolation_type, + lower_bound_step, + unused_previous, + upper_bound_step, + lower_bound_step.x, + upper_bound_step.x); + // No check on magnitude of step size being too small here as it is + // lower-bounded by the initial bracket start point, which was valid. + ++summary->num_function_evaluations; + if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; } + solution->value_is_valid = + function->Evaluate(solution->x, + &solution->value, + interpolation_uses_gradients + ? &solution->gradient : NULL); + solution->gradient_is_valid = + interpolation_uses_gradients && solution->value_is_valid; + if (!solution->value_is_valid) { + summary->error = + StringPrintf("Line search failed: Wolfe Zoom phase found " + "step_size: %.5e, for which function is invalid, " + "between low_step: %.5e and high_step: %.5e " + "at which function is valid.", + solution->x, bracket_low.x, bracket_high.x); + LOG(WARNING) << summary->error; + return false; + } + + if ((solution->value > (initial_position.value + + options().sufficient_decrease + * initial_position.gradient + * solution->x)) || + (solution->value >= bracket_low.value)) { + // Armijo sufficient decrease not satisfied, or not better + // than current lowest sample, use as new upper bound. + bracket_high = *solution; + continue; + } + + // Armijo sufficient decrease satisfied, check strong Wolfe condition. + if (!interpolation_uses_gradients) { + // Irrespective of the interpolation type we are using, we now need the + // gradient at the current point (which satisfies the Armijo condition) + // in order to check the strong Wolfe conditions. + ++summary->num_function_evaluations; + ++summary->num_gradient_evaluations; + solution->value_is_valid = + function->Evaluate(solution->x, + &solution->value, + &solution->gradient); + solution->gradient_is_valid = solution->value_is_valid; + if (!solution->value_is_valid) { + summary->error = + StringPrintf("Line search failed: Wolfe Zoom phase found " + "step_size: %.5e, for which function is invalid, " + "between low_step: %.5e and high_step: %.5e " + "at which function is valid.", + solution->x, bracket_low.x, bracket_high.x); + LOG(WARNING) << summary->error; + return false; + } + } + if (fabs(solution->gradient) <= + -options().sufficient_curvature_decrease * initial_position.gradient) { + // Found a valid termination point satisfying strong Wolfe conditions. + break; + + } else if (solution->gradient * (bracket_high.x - bracket_low.x) >= 0) { + bracket_high = bracket_low; + } + + bracket_low = *solution; + } + // Solution contains a valid point which satisfies the strong Wolfe + // conditions. + return true; +} + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search.h b/extern/libmv/third_party/ceres/internal/ceres/line_search.h index 95bf56e2a6b..5f24e9fd76e 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/line_search.h +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search.h @@ -35,15 +35,17 @@ #ifndef CERES_NO_LINE_SEARCH_MINIMIZER -#include <glog/logging.h> +#include <string> #include <vector> #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" +#include "ceres/types.h" namespace ceres { namespace internal { class Evaluator; +struct FunctionSample; // Line search is another name for a one dimensional optimization // algorithm. The name "line search" comes from the fact one @@ -61,32 +63,21 @@ class LineSearch { struct Options { Options() - : interpolation_degree(1), - use_higher_degree_interpolation_when_possible(false), + : interpolation_type(CUBIC), sufficient_decrease(1e-4), - min_relative_step_size_change(1e-3), - max_relative_step_size_change(0.6), - step_size_threshold(1e-9), + max_step_contraction(1e-3), + min_step_contraction(0.9), + min_step_size(1e-9), + max_num_iterations(20), + sufficient_curvature_decrease(0.9), + max_step_expansion(10.0), function(NULL) {} - // TODO(sameeragarwal): Replace this with enums which are common - // across various line searches. - // // Degree of the polynomial used to approximate the objective - // function. Valid values are {0, 1, 2}. - // - // For Armijo line search - // - // 0: Bisection based backtracking search. - // 1: Quadratic interpolation. - // 2: Cubic interpolation. - int interpolation_degree; - - // Usually its possible to increase the degree of the - // interpolation polynomial by storing and using an extra point. - bool use_higher_degree_interpolation_when_possible; + // function. + LineSearchInterpolationType interpolation_type; - // Armijo line search parameters. + // Armijo and Wolfe line search parameters. // Solving the line search problem exactly is computationally // prohibitive. Fortunately, line search based optimization @@ -99,19 +90,59 @@ class LineSearch { // f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size double sufficient_decrease; - // In each iteration of the Armijo line search, + // In each iteration of the Armijo / Wolfe line search, + // + // new_step_size >= max_step_contraction * step_size + // + // Note that by definition, for contraction: + // + // 0 < max_step_contraction < min_step_contraction < 1 // - // new_step_size >= min_relative_step_size_change * step_size - double min_relative_step_size_change; + double max_step_contraction; - // In each iteration of the Armijo line search, + // In each iteration of the Armijo / Wolfe line search, // - // new_step_size <= max_relative_step_size_change * step_size - double max_relative_step_size_change; + // new_step_size <= min_step_contraction * step_size + // Note that by definition, for contraction: + // + // 0 < max_step_contraction < min_step_contraction < 1 + // + double min_step_contraction; // If during the line search, the step_size falls below this // value, it is truncated to zero. - double step_size_threshold; + double min_step_size; + + // Maximum number of trial step size iterations during each line search, + // if a step size satisfying the search conditions cannot be found within + // this number of trials, the line search will terminate. + int max_num_iterations; + + // Wolfe-specific line search parameters. + + // The strong Wolfe conditions consist of the Armijo sufficient + // decrease condition, and an additional requirement that the + // step-size be chosen s.t. the _magnitude_ ('strong' Wolfe + // conditions) of the gradient along the search direction + // decreases sufficiently. Precisely, this second condition + // is that we seek a step_size s.t. + // + // |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)| + // + // Where f() is the line search objective and f'() is the derivative + // of f w.r.t step_size (d f / d step_size). + double sufficient_curvature_decrease; + + // During the bracketing phase of the Wolfe search, the step size is + // increased until either a point satisfying the Wolfe conditions is + // found, or an upper bound for a bracket containing a point satisfying + // the conditions is found. Precisely, at each iteration of the + // expansion: + // + // new_step_size <= max_step_expansion * step_size. + // + // By definition for expansion, max_step_expansion > 1.0. + double max_step_expansion; // The one dimensional function that the line search algorithm // minimizes. @@ -147,18 +178,28 @@ class LineSearch { Summary() : success(false), optimal_step_size(0.0), - num_evaluations(0) {} + num_function_evaluations(0), + num_gradient_evaluations(0), + num_iterations(0) {} bool success; double optimal_step_size; - int num_evaluations; + int num_function_evaluations; + int num_gradient_evaluations; + int num_iterations; + string error; }; + explicit LineSearch(const LineSearch::Options& options); virtual ~LineSearch() {} + static LineSearch* Create(const LineSearchType line_search_type, + const LineSearch::Options& options, + string* error); + // Perform the line search. // - // initial_step_size must be a positive number. + // step_size_estimate must be a positive number. // // initial_cost and initial_gradient are the values and gradient of // the function at zero. @@ -166,11 +207,23 @@ class LineSearch { // search. // // Summary::success is true if a non-zero step size is found. - virtual void Search(const LineSearch::Options& options, - double initial_step_size, + virtual void Search(double step_size_estimate, double initial_cost, double initial_gradient, Summary* summary) = 0; + double InterpolatingPolynomialMinimizingStepSize( + const LineSearchInterpolationType& interpolation_type, + const FunctionSample& lowerbound_sample, + const FunctionSample& previous_sample, + const FunctionSample& current_sample, + const double min_step_size, + const double max_step_size) const; + + protected: + const LineSearch::Options& options() const { return options_; } + + private: + LineSearch::Options options_; }; class LineSearchFunction : public LineSearch::Function { @@ -178,7 +231,8 @@ class LineSearchFunction : public LineSearch::Function { explicit LineSearchFunction(Evaluator* evaluator); virtual ~LineSearchFunction() {} void Init(const Vector& position, const Vector& direction); - virtual bool Evaluate(const double x, double* f, double* g); + virtual bool Evaluate(double x, double* f, double* g); + double DirectionInfinityNorm() const; private: Evaluator* evaluator_; @@ -200,12 +254,42 @@ class LineSearchFunction : public LineSearch::Function { // For more details: http://www.di.ens.fr/~mschmidt/Software/minFunc.html class ArmijoLineSearch : public LineSearch { public: + explicit ArmijoLineSearch(const LineSearch::Options& options); virtual ~ArmijoLineSearch() {} - virtual void Search(const LineSearch::Options& options, - double initial_step_size, + virtual void Search(double step_size_estimate, + double initial_cost, + double initial_gradient, + Summary* summary); +}; + +// Bracketing / Zoom Strong Wolfe condition line search. This implementation +// is based on the pseudo-code algorithm presented in Nocedal & Wright [1] +// (p60-61) with inspiration from the WolfeLineSearch which ships with the +// minFunc package by Mark Schmidt [2]. +// +// [1] Nocedal J., Wright S., Numerical Optimization, 2nd Ed., Springer, 1999. +// [2] http://www.di.ens.fr/~mschmidt/Software/minFunc.html. +class WolfeLineSearch : public LineSearch { + public: + explicit WolfeLineSearch(const LineSearch::Options& options); + virtual ~WolfeLineSearch() {} + virtual void Search(double step_size_estimate, double initial_cost, double initial_gradient, Summary* summary); + // Returns true iff either a valid point, or valid bracket are found. + bool BracketingPhase(const FunctionSample& initial_position, + const double step_size_estimate, + FunctionSample* bracket_low, + FunctionSample* bracket_high, + bool* perform_zoom_search, + Summary* summary); + // Returns true iff final_line_sample satisfies strong Wolfe conditions. + bool ZoomPhase(const FunctionSample& initial_position, + FunctionSample bracket_low, + FunctionSample bracket_high, + FunctionSample* solution, + Summary* summary); }; } // namespace internal diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.cc b/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.cc index b8b582c3fb1..8ded823e5bd 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.cc @@ -100,14 +100,24 @@ class NonlinearConjugateGradient : public LineSearchDirection { class LBFGS : public LineSearchDirection { public: - LBFGS(const int num_parameters, const int max_lbfgs_rank) - : low_rank_inverse_hessian_(num_parameters, max_lbfgs_rank) {} + LBFGS(const int num_parameters, + const int max_lbfgs_rank, + const bool use_approximate_eigenvalue_bfgs_scaling) + : low_rank_inverse_hessian_(num_parameters, + max_lbfgs_rank, + use_approximate_eigenvalue_bfgs_scaling), + is_positive_definite_(true) {} virtual ~LBFGS() {} bool NextDirection(const LineSearchMinimizer::State& previous, const LineSearchMinimizer::State& current, Vector* search_direction) { + CHECK(is_positive_definite_) + << "Ceres bug: NextDirection() called on L-BFGS after inverse Hessian " + << "approximation has become indefinite, please contact the " + << "developers!"; + low_rank_inverse_hessian_.Update( previous.search_direction * previous.step_size, current.gradient - previous.gradient); @@ -115,11 +125,177 @@ class LBFGS : public LineSearchDirection { low_rank_inverse_hessian_.RightMultiply(current.gradient.data(), search_direction->data()); *search_direction *= -1.0; + + if (search_direction->dot(current.gradient) >= 0.0) { + LOG(WARNING) << "Numerical failure in L-BFGS update: inverse Hessian " + << "approximation is not positive definite, and thus " + << "initial gradient for search direction is positive: " + << search_direction->dot(current.gradient); + is_positive_definite_ = false; + return false; + } + return true; } private: LowRankInverseHessian low_rank_inverse_hessian_; + bool is_positive_definite_; +}; + +class BFGS : public LineSearchDirection { + public: + BFGS(const int num_parameters, + const bool use_approximate_eigenvalue_scaling) + : num_parameters_(num_parameters), + use_approximate_eigenvalue_scaling_(use_approximate_eigenvalue_scaling), + initialized_(false), + is_positive_definite_(true) { + LOG_IF(WARNING, num_parameters_ >= 1e3) + << "BFGS line search being created with: " << num_parameters_ + << " parameters, this will allocate a dense approximate inverse Hessian" + << " of size: " << num_parameters_ << " x " << num_parameters_ + << ", consider using the L-BFGS memory-efficient line search direction " + << "instead."; + // Construct inverse_hessian_ after logging warning about size s.t. if the + // allocation crashes us, the log will highlight what the issue likely was. + inverse_hessian_ = Matrix::Identity(num_parameters, num_parameters); + } + + virtual ~BFGS() {} + + bool NextDirection(const LineSearchMinimizer::State& previous, + const LineSearchMinimizer::State& current, + Vector* search_direction) { + CHECK(is_positive_definite_) + << "Ceres bug: NextDirection() called on BFGS after inverse Hessian " + << "approximation has become indefinite, please contact the " + << "developers!"; + + const Vector delta_x = previous.search_direction * previous.step_size; + const Vector delta_gradient = current.gradient - previous.gradient; + const double delta_x_dot_delta_gradient = delta_x.dot(delta_gradient); + + if (delta_x_dot_delta_gradient <= 1e-10) { + VLOG(2) << "Skipping BFGS Update, delta_x_dot_delta_gradient too " + << "small: " << delta_x_dot_delta_gradient; + } else { + // Update dense inverse Hessian approximation. + + if (!initialized_ && use_approximate_eigenvalue_scaling_) { + // Rescale the initial inverse Hessian approximation (H_0) to be + // iteratively updated so that it is of similar 'size' to the true + // inverse Hessian at the start point. As shown in [1]: + // + // \gamma = (delta_gradient_{0}' * delta_x_{0}) / + // (delta_gradient_{0}' * delta_gradient_{0}) + // + // Satisfies: + // + // (1 / \lambda_m) <= \gamma <= (1 / \lambda_1) + // + // Where \lambda_1 & \lambda_m are the smallest and largest eigenvalues + // of the true initial Hessian (not the inverse) respectively. Thus, + // \gamma is an approximate eigenvalue of the true inverse Hessian, and + // choosing: H_0 = I * \gamma will yield a starting point that has a + // similar scale to the true inverse Hessian. This technique is widely + // reported to often improve convergence, however this is not + // universally true, particularly if there are errors in the initial + // gradients, or if there are significant differences in the sensitivity + // of the problem to the parameters (i.e. the range of the magnitudes of + // the components of the gradient is large). + // + // The original origin of this rescaling trick is somewhat unclear, the + // earliest reference appears to be Oren [1], however it is widely + // discussed without specific attributation in various texts including + // [2] (p143). + // + // [1] Oren S.S., Self-scaling variable metric (SSVM) algorithms + // Part II: Implementation and experiments, Management Science, + // 20(5), 863-874, 1974. + // [2] Nocedal J., Wright S., Numerical Optimization, Springer, 1999. + inverse_hessian_ *= + delta_x_dot_delta_gradient / delta_gradient.dot(delta_gradient); + } + initialized_ = true; + + // Efficient O(num_parameters^2) BFGS update [2]. + // + // Starting from dense BFGS update detailed in Nocedal [2] p140/177 and + // using: y_k = delta_gradient, s_k = delta_x: + // + // \rho_k = 1.0 / (s_k' * y_k) + // V_k = I - \rho_k * y_k * s_k' + // H_k = (V_k' * H_{k-1} * V_k) + (\rho_k * s_k * s_k') + // + // This update involves matrix, matrix products which naively O(N^3), + // however we can exploit our knowledge that H_k is positive definite + // and thus by defn. symmetric to reduce the cost of the update: + // + // Expanding the update above yields: + // + // H_k = H_{k-1} + + // \rho_k * ( (1.0 + \rho_k * y_k' * H_k * y_k) * s_k * s_k' - + // (s_k * y_k' * H_k + H_k * y_k * s_k') ) + // + // Using: A = (s_k * y_k' * H_k), and the knowledge that H_k = H_k', the + // last term simplifies to (A + A'). Note that although A is not symmetric + // (A + A') is symmetric. For ease of construction we also define + // B = (1 + \rho_k * y_k' * H_k * y_k) * s_k * s_k', which is by defn + // symmetric due to construction from: s_k * s_k'. + // + // Now we can write the BFGS update as: + // + // H_k = H_{k-1} + \rho_k * (B - (A + A')) + + // For efficiency, as H_k is by defn. symmetric, we will only maintain the + // *lower* triangle of H_k (and all intermediary terms). + + const double rho_k = 1.0 / delta_x_dot_delta_gradient; + + // Calculate: A = s_k * y_k' * H_k + Matrix A = delta_x * (delta_gradient.transpose() * + inverse_hessian_.selfadjointView<Eigen::Lower>()); + + // Calculate scalar: (1 + \rho_k * y_k' * H_k * y_k) + const double delta_x_times_delta_x_transpose_scale_factor = + (1.0 + (rho_k * delta_gradient.transpose() * + inverse_hessian_.selfadjointView<Eigen::Lower>() * + delta_gradient)); + // Calculate: B = (1 + \rho_k * y_k' * H_k * y_k) * s_k * s_k' + Matrix B = Matrix::Zero(num_parameters_, num_parameters_); + B.selfadjointView<Eigen::Lower>(). + rankUpdate(delta_x, delta_x_times_delta_x_transpose_scale_factor); + + // Finally, update inverse Hessian approximation according to: + // H_k = H_{k-1} + \rho_k * (B - (A + A')). Note that (A + A') is + // symmetric, even though A is not. + inverse_hessian_.triangularView<Eigen::Lower>() += + rho_k * (B - A - A.transpose()); + } + + *search_direction = + inverse_hessian_.selfadjointView<Eigen::Lower>() * + (-1.0 * current.gradient); + + if (search_direction->dot(current.gradient) >= 0.0) { + LOG(WARNING) << "Numerical failure in BFGS update: inverse Hessian " + << "approximation is not positive definite, and thus " + << "initial gradient for search direction is positive: " + << search_direction->dot(current.gradient); + is_positive_definite_ = false; + return false; + } + + return true; + } + + private: + const int num_parameters_; + const bool use_approximate_eigenvalue_scaling_; + Matrix inverse_hessian_; + bool initialized_; + bool is_positive_definite_; }; LineSearchDirection* @@ -135,8 +311,16 @@ LineSearchDirection::Create(const LineSearchDirection::Options& options) { } if (options.type == ceres::LBFGS) { - return new ceres::internal::LBFGS(options.num_parameters, - options.max_lbfgs_rank); + return new ceres::internal::LBFGS( + options.num_parameters, + options.max_lbfgs_rank, + options.use_approximate_eigenvalue_bfgs_scaling); + } + + if (options.type == ceres::BFGS) { + return new ceres::internal::BFGS( + options.num_parameters, + options.use_approximate_eigenvalue_bfgs_scaling); } LOG(ERROR) << "Unknown line search direction type: " << options.type; diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.h b/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.h index 08747544bbe..0857cb005f9 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.h +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.h @@ -48,7 +48,8 @@ class LineSearchDirection { type(LBFGS), nonlinear_conjugate_gradient_type(FLETCHER_REEVES), function_tolerance(1e-12), - max_lbfgs_rank(20) { + max_lbfgs_rank(20), + use_approximate_eigenvalue_bfgs_scaling(true) { } int num_parameters; @@ -56,6 +57,7 @@ class LineSearchDirection { NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; double function_tolerance; int max_lbfgs_rank; + bool use_approximate_eigenvalue_bfgs_scaling; }; static LineSearchDirection* Create(const Options& options); diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search_minimizer.cc b/extern/libmv/third_party/ceres/internal/ceres/line_search_minimizer.cc index 684a7369b3a..2cc89faf4c4 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/line_search_minimizer.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search_minimizer.cc @@ -160,17 +160,44 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, line_search_direction_options.nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; line_search_direction_options.max_lbfgs_rank = options.max_lbfgs_rank; + line_search_direction_options.use_approximate_eigenvalue_bfgs_scaling = + options.use_approximate_eigenvalue_bfgs_scaling; scoped_ptr<LineSearchDirection> line_search_direction( LineSearchDirection::Create(line_search_direction_options)); LineSearchFunction line_search_function(evaluator); + LineSearch::Options line_search_options; + line_search_options.interpolation_type = + options.line_search_interpolation_type; + line_search_options.min_step_size = options.min_line_search_step_size; + line_search_options.sufficient_decrease = + options.line_search_sufficient_function_decrease; + line_search_options.max_step_contraction = + options.max_line_search_step_contraction; + line_search_options.min_step_contraction = + options.min_line_search_step_contraction; + line_search_options.max_num_iterations = + options.max_num_line_search_step_size_iterations; + line_search_options.sufficient_curvature_decrease = + options.line_search_sufficient_curvature_decrease; + line_search_options.max_step_expansion = + options.max_line_search_step_expansion; line_search_options.function = &line_search_function; - // TODO(sameeragarwal): Make this parameterizable over different - // line searches. - ArmijoLineSearch line_search; + scoped_ptr<LineSearch> + line_search(LineSearch::Create(options.line_search_type, + line_search_options, + &summary->error)); + if (line_search.get() == NULL) { + LOG(ERROR) << "Ceres bug: Unable to create a LineSearch object, please " + << "contact the developers!, error: " << summary->error; + summary->termination_type = DID_NOT_RUN; + return; + } + LineSearch::Summary line_search_summary; + int num_line_search_direction_restarts = 0; while (true) { if (!RunCallbacks(options.callbacks, iteration_summary, summary)) { @@ -194,6 +221,8 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, iteration_summary = IterationSummary(); iteration_summary.iteration = summary->iterations.back().iteration + 1; + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; bool line_search_status = true; if (iteration_summary.iteration == 1) { @@ -205,9 +234,36 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, ¤t_state.search_direction); } - if (!line_search_status) { - LOG(WARNING) << "Line search direction computation failed. " - "Resorting to steepest descent."; + if (!line_search_status && + num_line_search_direction_restarts >= + options.max_num_line_search_direction_restarts) { + // Line search direction failed to generate a new direction, and we + // have already reached our specified maximum number of restarts, + // terminate optimization. + summary->error = + StringPrintf("Line search direction failure: specified " + "max_num_line_search_direction_restarts: %d reached.", + options.max_num_line_search_direction_restarts); + LOG(WARNING) << summary->error << " terminating optimization."; + summary->termination_type = NUMERICAL_FAILURE; + break; + + } else if (!line_search_status) { + // Restart line search direction with gradient descent on first iteration + // as we have not yet reached our maximum number of restarts. + CHECK_LT(num_line_search_direction_restarts, + options.max_num_line_search_direction_restarts); + + ++num_line_search_direction_restarts; + LOG(WARNING) + << "Line search direction algorithm: " + << LineSearchDirectionTypeToString(options.line_search_direction_type) + << ", failed to produce a valid new direction at iteration: " + << iteration_summary.iteration << ". Restarting, number of " + << "restarts: " << num_line_search_direction_restarts << " / " + << options.max_num_line_search_direction_restarts << " [max]."; + line_search_direction.reset( + LineSearchDirection::Create(line_search_direction_options)); current_state.search_direction = -current_state.gradient; } @@ -217,21 +273,41 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, // TODO(sameeragarwal): Refactor this into its own object and add // explanations for the various choices. - const double initial_step_size = (iteration_summary.iteration == 1) + // + // Note that we use !line_search_status to ensure that we treat cases when + // we restarted the line search direction equivalently to the first + // iteration. + const double initial_step_size = + (iteration_summary.iteration == 1 || !line_search_status) ? min(1.0, 1.0 / current_state.gradient_max_norm) : min(1.0, 2.0 * (current_state.cost - previous_state.cost) / current_state.directional_derivative); + // By definition, we should only ever go forwards along the specified search + // direction in a line search, most likely cause for this being violated + // would be a numerical failure in the line search direction calculation. + if (initial_step_size < 0.0) { + summary->error = + StringPrintf("Numerical failure in line search, initial_step_size is " + "negative: %.5e, directional_derivative: %.5e, " + "(current_cost - previous_cost): %.5e", + initial_step_size, current_state.directional_derivative, + (current_state.cost - previous_state.cost)); + LOG(WARNING) << summary->error; + summary->termination_type = NUMERICAL_FAILURE; + break; + } - line_search.Search(line_search_options, - initial_step_size, - current_state.cost, - current_state.directional_derivative, - &line_search_summary); + line_search->Search(initial_step_size, + current_state.cost, + current_state.directional_derivative, + &line_search_summary); current_state.step_size = line_search_summary.optimal_step_size; delta = current_state.step_size * current_state.search_direction; previous_state = current_state; + iteration_summary.step_solver_time_in_seconds = + WallTimeInSeconds() - iteration_start_time; // TODO(sameeragarwal): Collect stats. if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data()) || @@ -270,7 +346,11 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, iteration_summary.step_norm = delta.norm(); iteration_summary.step_size = current_state.step_size; iteration_summary.line_search_function_evaluations = - line_search_summary.num_evaluations; + line_search_summary.num_function_evaluations; + iteration_summary.line_search_gradient_evaluations = + line_search_summary.num_gradient_evaluations; + iteration_summary.line_search_iterations = + line_search_summary.num_iterations; iteration_summary.iteration_time_in_seconds = WallTimeInSeconds() - iteration_start_time; iteration_summary.cumulative_time_in_seconds = @@ -278,6 +358,7 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, + summary->preprocessor_time_in_seconds; summary->iterations.push_back(iteration_summary); + ++summary->num_successful_steps; } } 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 6c886a1be38..24ba565daf9 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 @@ -36,10 +36,8 @@ #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/stringprintf.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" @@ -64,74 +62,6 @@ LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) { return NULL; } -#ifndef CERES_NO_PROTOCOL_BUFFERS -LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( - const string& filename) { - LinearLeastSquaresProblemProto problem_proto; - { - string serialized_proto; - ReadFileToStringOrDie(filename, &serialized_proto); - CHECK(problem_proto.ParseFromString(serialized_proto)); - } - - LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; - const SparseMatrixProto& A = problem_proto.a(); - - if (A.has_block_matrix()) { - problem->A.reset(new BlockSparseMatrix(A)); - } else if (A.has_triplet_matrix()) { - problem->A.reset(new TripletSparseMatrix(A)); - } else { - problem->A.reset(new CompressedRowSparseMatrix(A)); - } - - if (problem_proto.b_size() > 0) { - problem->b.reset(new double[problem_proto.b_size()]); - for (int i = 0; i < problem_proto.b_size(); ++i) { - problem->b[i] = problem_proto.b(i); - } - } - - if (problem_proto.d_size() > 0) { - problem->D.reset(new double[problem_proto.d_size()]); - for (int i = 0; i < problem_proto.d_size(); ++i) { - problem->D[i] = problem_proto.d(i); - } - } - - if (problem_proto.d_size() > 0) { - if (problem_proto.x_size() > 0) { - problem->x_D.reset(new double[problem_proto.x_size()]); - for (int i = 0; i < problem_proto.x_size(); ++i) { - problem->x_D[i] = problem_proto.x(i); - } - } - } else { - if (problem_proto.x_size() > 0) { - problem->x.reset(new double[problem_proto.x_size()]); - for (int i = 0; i < problem_proto.x_size(); ++i) { - problem->x[i] = problem_proto.x(i); - } - } - } - - problem->num_eliminate_blocks = 0; - if (problem_proto.has_num_eliminate_blocks()) { - problem->num_eliminate_blocks = problem_proto.num_eliminate_blocks(); - } - - return problem; -} -#else -LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( - const string& filename) { - LOG(FATAL) - << "Loading a least squares problem from disk requires " - << "Ceres to be built with Protocol Buffers support."; - return NULL; -} -#endif // CERES_NO_PROTOCOL_BUFFERS - /* A = [1 2] [3 4] @@ -574,9 +504,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem3() { } namespace { -bool DumpLinearLeastSquaresProblemToConsole(const string& directory, - int iteration, - const SparseMatrix* A, +bool DumpLinearLeastSquaresProblemToConsole(const SparseMatrix* A, const double* D, const double* b, const double* x, @@ -601,61 +529,6 @@ bool DumpLinearLeastSquaresProblemToConsole(const string& directory, return true; }; -#ifndef CERES_NO_PROTOCOL_BUFFERS -bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory, - int iteration, - const SparseMatrix* A, - const double* D, - const double* b, - const double* x, - int num_eliminate_blocks) { - CHECK_NOTNULL(A); - LinearLeastSquaresProblemProto lsqp; - A->ToProto(lsqp.mutable_a()); - - if (D != NULL) { - for (int i = 0; i < A->num_cols(); ++i) { - lsqp.add_d(D[i]); - } - } - - if (b != NULL) { - for (int i = 0; i < A->num_rows(); ++i) { - lsqp.add_b(b[i]); - } - } - - if (x != NULL) { - for (int i = 0; i < A->num_cols(); ++i) { - lsqp.add_x(x[i]); - } - } - - lsqp.set_num_eliminate_blocks(num_eliminate_blocks); - string format_string = JoinPath(directory, - "lm_iteration_%03d.lsqp"); - string filename = - StringPrintf(format_string.c_str(), iteration); - LOG(INFO) << "Dumping least squares problem for iteration " << iteration - << " to disk. File: " << filename; - WriteStringToFileOrDie(lsqp.SerializeAsString(), filename); - return true; -} -#else -bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory, - int iteration, - const SparseMatrix* A, - const double* D, - const double* b, - const double* x, - int num_eliminate_blocks) { - LOG(ERROR) << "Dumping least squares problems is only " - << "supported when Ceres is compiled with " - << "protocol buffer support."; - return false; -} -#endif - void WriteArrayToFileOrDie(const string& filename, const double* x, const int size) { @@ -669,31 +542,25 @@ void WriteArrayToFileOrDie(const string& filename, fclose(fptr); } -bool DumpLinearLeastSquaresProblemToTextFile(const string& directory, - int iteration, +bool DumpLinearLeastSquaresProblemToTextFile(const string& filename_base, const SparseMatrix* A, const double* D, const double* b, const double* x, int num_eliminate_blocks) { CHECK_NOTNULL(A); - string format_string = JoinPath(directory, - "lm_iteration_%03d"); - string filename_prefix = - StringPrintf(format_string.c_str(), iteration); - - LOG(INFO) << "writing to: " << filename_prefix << "*"; + LOG(INFO) << "writing to: " << filename_base << "*"; string matlab_script; StringAppendF(&matlab_script, - "function lsqp = lm_iteration_%03d()\n", iteration); + "function lsqp = load_trust_region_problem()\n"); 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"; + string filename = filename_base + "_A.txt"; FILE* fptr = fopen(filename.c_str(), "w"); CHECK_NOTNULL(fptr); A->ToTextFile(fptr); @@ -709,34 +576,33 @@ bool DumpLinearLeastSquaresProblemToTextFile(const string& directory, if (D != NULL) { - string filename = filename_prefix + "_D.txt"; + string filename = filename_base + "_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"; + string filename = filename_base + "_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"; + string filename = filename_base + "_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"; + string matlab_filename = filename_base + ".m"; WriteStringToFileOrDie(matlab_script, matlab_filename); return true; } } // namespace -bool DumpLinearLeastSquaresProblem(const string& directory, - int iteration, +bool DumpLinearLeastSquaresProblem(const string& filename_base, DumpFormatType dump_format_type, const SparseMatrix* A, const double* D, @@ -745,19 +611,10 @@ bool DumpLinearLeastSquaresProblem(const string& directory, int num_eliminate_blocks) { switch (dump_format_type) { case CONSOLE: - return DumpLinearLeastSquaresProblemToConsole(directory, - iteration, - A, D, b, x, + return DumpLinearLeastSquaresProblemToConsole(A, D, b, x, num_eliminate_blocks); - case PROTOBUF: - return DumpLinearLeastSquaresProblemToProtocolBuffer( - directory, - iteration, - A, D, b, x, - num_eliminate_blocks); case TEXTFILE: - return DumpLinearLeastSquaresProblemToTextFile(directory, - iteration, + return DumpLinearLeastSquaresProblemToTextFile(filename_base, A, D, b, x, num_eliminate_blocks); default: diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h index c76ae91c7d8..fdeed70de62 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h @@ -63,8 +63,6 @@ struct LinearLeastSquaresProblem { // Factories for linear least squares problem. LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id); -LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( - const string& filename); LinearLeastSquaresProblem* LinearLeastSquaresProblem0(); LinearLeastSquaresProblem* LinearLeastSquaresProblem1(); @@ -73,8 +71,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem3(); // Write the linear least squares problem to disk. The exact format // depends on dump_format_type. -bool DumpLinearLeastSquaresProblem(const string& directory, - int iteration, +bool DumpLinearLeastSquaresProblem(const string& filename_base, DumpFormatType dump_format_type, const SparseMatrix* A, const double* D, 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 ca10faa24b4..22691b33e44 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h @@ -36,6 +36,7 @@ #include <cstddef> #include <map> +#include <string> #include <vector> #include "ceres/block_sparse_matrix.h" #include "ceres/casts.h" @@ -73,7 +74,8 @@ class LinearSolver { Options() : type(SPARSE_NORMAL_CHOLESKY), preconditioner_type(JACOBI), - sparse_linear_algebra_library(SUITE_SPARSE), + dense_linear_algebra_library_type(EIGEN), + sparse_linear_algebra_library_type(SUITE_SPARSE), use_postordering(false), min_num_iterations(1), max_num_iterations(1), @@ -88,7 +90,8 @@ class LinearSolver { PreconditionerType preconditioner_type; - SparseLinearAlgebraLibraryType sparse_linear_algebra_library; + DenseLinearAlgebraLibraryType dense_linear_algebra_library_type; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; // See solver.h for information about this flag. bool use_postordering; @@ -316,7 +319,6 @@ class TypedLinearSolver : public LinearSolver { // Linear solvers that depend on acccess to the low level structure of // a SparseMatrix. typedef TypedLinearSolver<BlockSparseMatrix> BlockSparseMatrixSolver; // NOLINT -typedef TypedLinearSolver<BlockSparseMatrixBase> BlockSparseMatrixBaseSolver; // NOLINT typedef TypedLinearSolver<CompressedRowSparseMatrix> CompressedRowSparseMatrixSolver; // NOLINT typedef TypedLinearSolver<DenseSparseMatrix> DenseSparseMatrixSolver; // NOLINT typedef TypedLinearSolver<TripletSparseMatrix> TripletSparseMatrixSolver; // NOLINT diff --git a/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.cc b/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.cc index 3fe113f1afb..372165f9523 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.cc @@ -35,12 +35,15 @@ namespace ceres { namespace internal { -LowRankInverseHessian::LowRankInverseHessian(int num_parameters, - int max_num_corrections) +LowRankInverseHessian::LowRankInverseHessian( + int num_parameters, + int max_num_corrections, + bool use_approximate_eigenvalue_scaling) : num_parameters_(num_parameters), max_num_corrections_(max_num_corrections), + use_approximate_eigenvalue_scaling_(use_approximate_eigenvalue_scaling), num_corrections_(0), - diagonal_(1.0), + approximate_eigenvalue_scale_(1.0), delta_x_history_(num_parameters, max_num_corrections), delta_gradient_history_(num_parameters, max_num_corrections), delta_x_dot_delta_gradient_(max_num_corrections) { @@ -50,7 +53,8 @@ bool LowRankInverseHessian::Update(const Vector& delta_x, const Vector& delta_gradient) { const double delta_x_dot_delta_gradient = delta_x.dot(delta_gradient); if (delta_x_dot_delta_gradient <= 1e-10) { - VLOG(2) << "Skipping LBFGS Update. " << delta_x_dot_delta_gradient; + VLOG(2) << "Skipping LBFGS Update, delta_x_dot_delta_gradient too small: " + << delta_x_dot_delta_gradient; return false; } @@ -58,16 +62,16 @@ bool LowRankInverseHessian::Update(const Vector& delta_x, // TODO(sameeragarwal): This can be done more efficiently using // a circular buffer/indexing scheme, but for simplicity we will // do the expensive copy for now. - delta_x_history_.block(0, 0, num_parameters_, max_num_corrections_ - 2) = + delta_x_history_.block(0, 0, num_parameters_, max_num_corrections_ - 1) = delta_x_history_ .block(0, 1, num_parameters_, max_num_corrections_ - 1); delta_gradient_history_ - .block(0, 0, num_parameters_, max_num_corrections_ - 2) = + .block(0, 0, num_parameters_, max_num_corrections_ - 1) = delta_gradient_history_ .block(0, 1, num_parameters_, max_num_corrections_ - 1); - delta_x_dot_delta_gradient_.head(num_corrections_ - 2) = + delta_x_dot_delta_gradient_.head(num_corrections_ - 1) = delta_x_dot_delta_gradient_.tail(num_corrections_ - 1); } else { ++num_corrections_; @@ -77,7 +81,8 @@ bool LowRankInverseHessian::Update(const Vector& delta_x, delta_gradient_history_.col(num_corrections_ - 1) = delta_gradient; delta_x_dot_delta_gradient_(num_corrections_ - 1) = delta_x_dot_delta_gradient; - diagonal_ = delta_x_dot_delta_gradient / delta_gradient.squaredNorm(); + approximate_eigenvalue_scale_ = + delta_x_dot_delta_gradient / delta_gradient.squaredNorm(); return true; } @@ -96,7 +101,39 @@ void LowRankInverseHessian::RightMultiply(const double* x_ptr, search_direction -= alpha(i) * delta_gradient_history_.col(i); } - search_direction *= diagonal_; + if (use_approximate_eigenvalue_scaling_) { + // Rescale the initial inverse Hessian approximation (H_0) to be iteratively + // updated so that it is of similar 'size' to the true inverse Hessian along + // the most recent search direction. As shown in [1]: + // + // \gamma_k = (delta_gradient_{k-1}' * delta_x_{k-1}) / + // (delta_gradient_{k-1}' * delta_gradient_{k-1}) + // + // Satisfies: + // + // (1 / \lambda_m) <= \gamma_k <= (1 / \lambda_1) + // + // Where \lambda_1 & \lambda_m are the smallest and largest eigenvalues of + // the true Hessian (not the inverse) along the most recent search direction + // respectively. Thus \gamma is an approximate eigenvalue of the true + // inverse Hessian, and choosing: H_0 = I * \gamma will yield a starting + // point that has a similar scale to the true inverse Hessian. This + // technique is widely reported to often improve convergence, however this + // is not universally true, particularly if there are errors in the initial + // jacobians, or if there are significant differences in the sensitivity + // of the problem to the parameters (i.e. the range of the magnitudes of + // the components of the gradient is large). + // + // The original origin of this rescaling trick is somewhat unclear, the + // earliest reference appears to be Oren [1], however it is widely discussed + // without specific attributation in various texts including [2] (p143/178). + // + // [1] Oren S.S., Self-scaling variable metric (SSVM) algorithms Part II: + // Implementation and experiments, Management Science, + // 20(5), 863-874, 1974. + // [2] Nocedal J., Wright S., Numerical Optimization, Springer, 1999. + search_direction *= approximate_eigenvalue_scale_; + } for (int i = 0; i < num_corrections_; ++i) { const double beta = delta_gradient_history_.col(i).dot(search_direction) / diff --git a/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.h b/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.h index 6f3fc0c9d00..7d293d09422 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.h +++ b/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.h @@ -61,10 +61,16 @@ class LowRankInverseHessian : public LinearOperator { public: // num_parameters is the row/column size of the Hessian. // max_num_corrections is the rank of the Hessian approximation. + // use_approximate_eigenvalue_scaling controls whether the initial + // inverse Hessian used during Right/LeftMultiply() is scaled by + // the approximate eigenvalue of the true inverse Hessian at the + // current operating point. // The approximation uses: // 2 * max_num_corrections * num_parameters + max_num_corrections // doubles. - LowRankInverseHessian(int num_parameters, int max_num_corrections); + LowRankInverseHessian(int num_parameters, + int max_num_corrections, + bool use_approximate_eigenvalue_scaling); virtual ~LowRankInverseHessian() {} // Update the low rank approximation. delta_x is the change in the @@ -86,8 +92,9 @@ class LowRankInverseHessian : public LinearOperator { private: const int num_parameters_; const int max_num_corrections_; + const bool use_approximate_eigenvalue_scaling_; int num_corrections_; - double diagonal_; + double approximate_eigenvalue_scale_; Matrix delta_x_history_; Matrix delta_gradient_history_; Vector delta_x_dot_delta_gradient_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/minimizer.h b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h index 040ddd96fbb..622e9cee1d0 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/minimizer.h +++ b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h @@ -31,6 +31,7 @@ #ifndef CERES_INTERNAL_MINIMIZER_H_ #define CERES_INTERNAL_MINIMIZER_H_ +#include <string> #include <vector> #include "ceres/internal/port.h" #include "ceres/iteration_callback.h" @@ -73,9 +74,12 @@ class Minimizer { 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; + trust_region_problem_dump_directory = + options.trust_region_problem_dump_directory; + trust_region_minimizer_iterations_to_dump = + options.trust_region_minimizer_iterations_to_dump; + trust_region_problem_dump_format_type = + options.trust_region_problem_dump_format_type; max_num_consecutive_invalid_steps = options.max_num_consecutive_invalid_steps; min_trust_region_radius = options.min_trust_region_radius; @@ -84,11 +88,31 @@ class Minimizer { nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; max_lbfgs_rank = options.max_lbfgs_rank; + use_approximate_eigenvalue_bfgs_scaling = + options.use_approximate_eigenvalue_bfgs_scaling; + line_search_interpolation_type = + options.line_search_interpolation_type; + min_line_search_step_size = options.min_line_search_step_size; + line_search_sufficient_function_decrease = + options.line_search_sufficient_function_decrease; + max_line_search_step_contraction = + options.max_line_search_step_contraction; + min_line_search_step_contraction = + options.min_line_search_step_contraction; + max_num_line_search_step_size_iterations = + options.max_num_line_search_step_size_iterations; + max_num_line_search_direction_restarts = + options.max_num_line_search_direction_restarts; + line_search_sufficient_curvature_decrease = + options.line_search_sufficient_curvature_decrease; + max_line_search_step_expansion = + options.max_line_search_step_expansion; evaluator = NULL; trust_region_strategy = NULL; jacobian = NULL; callbacks = options.callbacks; inner_iteration_minimizer = NULL; + inner_iteration_tolerance = options.inner_iteration_tolerance; } int max_num_iterations; @@ -109,15 +133,26 @@ class Minimizer { bool jacobi_scaling; bool use_nonmonotonic_steps; int max_consecutive_nonmonotonic_steps; - vector<int> lsqp_iterations_to_dump; - DumpFormatType lsqp_dump_format_type; - string lsqp_dump_directory; + vector<int> trust_region_minimizer_iterations_to_dump; + DumpFormatType trust_region_problem_dump_format_type; + string trust_region_problem_dump_directory; int max_num_consecutive_invalid_steps; double min_trust_region_radius; LineSearchDirectionType line_search_direction_type; LineSearchType line_search_type; NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; int max_lbfgs_rank; + bool use_approximate_eigenvalue_bfgs_scaling; + LineSearchInterpolationType line_search_interpolation_type; + double min_line_search_step_size; + double line_search_sufficient_function_decrease; + double max_line_search_step_contraction; + double min_line_search_step_contraction; + int max_num_line_search_step_size_iterations; + int max_num_line_search_direction_restarts; + double line_search_sufficient_curvature_decrease; + double max_line_search_step_expansion; + // List of callbacks that are executed by the Minimizer at the end // of each iteration. @@ -141,6 +176,7 @@ class Minimizer { SparseMatrix* jacobian; Minimizer* inner_iteration_minimizer; + double inner_iteration_tolerance; }; static bool RunCallbacks(const vector<IterationCallback*> callbacks, 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 b1e8d938b8a..695fa6ff97b 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h +++ b/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h @@ -173,8 +173,8 @@ class ParameterBlock { new double[local_parameterization_->GlobalSize() * local_parameterization_->LocalSize()]); CHECK(UpdateLocalParameterizationJacobian()) - "Local parameterization Jacobian computation failed" - "for x: " << ConstVectorRef(state_, Size()).transpose(); + << "Local parameterization Jacobian computation failed for x: " + << ConstVectorRef(state_, Size()).transpose(); } else { // Ignore the case that the parameterizations match. } diff --git a/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.cc b/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.cc index e8f626f8e80..190715bee43 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.cc @@ -42,6 +42,32 @@ namespace ceres { namespace internal { +int ComputeStableSchurOrdering(const Program& program, + vector<ParameterBlock*>* ordering) { + CHECK_NOTNULL(ordering)->clear(); + + scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + const HashSet<ParameterBlock*>& vertices = graph->vertices(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + if (vertices.count(parameter_blocks[i]) > 0) { + ordering->push_back(parameter_blocks[i]); + } + } + + int independent_set_size = StableIndependentSetOrdering(*graph, ordering); + + // Add the excluded blocks to back of the ordering vector. + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + if (parameter_block->IsConstant()) { + ordering->push_back(parameter_block); + } + } + + return independent_set_size; +} + int ComputeSchurOrdering(const Program& program, vector<ParameterBlock*>* ordering) { CHECK_NOTNULL(ordering)->clear(); diff --git a/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.h b/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.h index a5277a44c70..4675cb8dc7c 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.h +++ b/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.h @@ -58,6 +58,12 @@ class ParameterBlock; int ComputeSchurOrdering(const Program& program, vector<ParameterBlock* >* ordering); +// Same as above, except that ties while computing the independent set +// ordering are resolved in favour of the order in which the parameter +// blocks occur in the program. +int ComputeStableSchurOrdering(const Program& program, + vector<ParameterBlock* >* ordering); + // Use an approximate independent set ordering to decompose the // parameter blocks of a problem in a sequence of independent // sets. The ordering covers all the non-constant parameter blocks in 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 c488184ac93..59eaff8ec1b 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,17 +35,17 @@ #include <algorithm> #include <cstring> #include <vector> -#include "ceres/blas.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" #include "ceres/internal/eigen.h" +#include "ceres/small_blas.h" #include "glog/logging.h" namespace ceres { namespace internal { PartitionedMatrixView::PartitionedMatrixView( - const BlockSparseMatrixBase& matrix, + const BlockSparseMatrix& matrix, int num_col_blocks_a) : matrix_(matrix), num_col_blocks_e_(num_col_blocks_a) { @@ -96,8 +96,8 @@ void PartitionedMatrixView::RightMultiplyE(const double* x, double* y) const { // Iterate over the first num_row_blocks_e_ row blocks, and multiply // by the first cell in each row block. + const double* values = matrix_.values(); for (int r = 0; r < num_row_blocks_e_; ++r) { - const double* row_values = matrix_.RowBlockValues(r); const Cell& cell = bs->rows[r].cells[0]; const int row_block_pos = bs->rows[r].block.position; const int row_block_size = bs->rows[r].block.size; @@ -105,7 +105,7 @@ void PartitionedMatrixView::RightMultiplyE(const double* x, double* y) const { const int col_block_pos = bs->cols[col_block_id].position; const int col_block_size = bs->cols[col_block_id].size; MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cell.position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, x + col_block_pos, y + row_block_pos); } @@ -119,17 +119,17 @@ void PartitionedMatrixView::RightMultiplyF(const double* x, double* y) const { // E. If the row block is not in E (i.e its in the bottom // num_row_blocks - num_row_blocks_e row blocks), then all the cells // are of type F and multiply by them all. + const double* values = matrix_.values(); for (int r = 0; r < bs->rows.size(); ++r) { const int row_block_pos = bs->rows[r].block.position; const int row_block_size = bs->rows[r].block.size; const vector<Cell>& cells = bs->rows[r].cells; for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { - const double* row_values = matrix_.RowBlockValues(r); const int col_block_id = cells[c].block_id; const int col_block_pos = bs->cols[col_block_id].position; const int col_block_size = bs->cols[col_block_id].size; MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cells[c].position, row_block_size, col_block_size, + values + cells[c].position, row_block_size, col_block_size, x + col_block_pos - num_cols_e(), y + row_block_pos); } @@ -141,16 +141,16 @@ void PartitionedMatrixView::LeftMultiplyE(const double* x, double* y) const { // Iterate over the first num_row_blocks_e_ row blocks, and multiply // by the first cell in each row block. + const double* values = matrix_.values(); for (int r = 0; r < num_row_blocks_e_; ++r) { const Cell& cell = bs->rows[r].cells[0]; - const double* row_values = matrix_.RowBlockValues(r); const int row_block_pos = bs->rows[r].block.position; const int row_block_size = bs->rows[r].block.size; const int col_block_id = cell.block_id; const int col_block_pos = bs->cols[col_block_id].position; const int col_block_size = bs->cols[col_block_id].size; MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cell.position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, x + row_block_pos, y + col_block_pos); } @@ -164,17 +164,17 @@ void PartitionedMatrixView::LeftMultiplyF(const double* x, double* y) const { // E. If the row block is not in E (i.e its in the bottom // num_row_blocks - num_row_blocks_e row blocks), then all the cells // are of type F and multiply by them all. + const double* values = matrix_.values(); for (int r = 0; r < bs->rows.size(); ++r) { const int row_block_pos = bs->rows[r].block.position; const int row_block_size = bs->rows[r].block.size; const vector<Cell>& cells = bs->rows[r].cells; for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { - const double* row_values = matrix_.RowBlockValues(r); const int col_block_id = cells[c].block_id; const int col_block_pos = bs->cols[col_block_id].position; const int col_block_size = bs->cols[col_block_id].size; MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cells[c].position, row_block_size, col_block_size, + values + cells[c].position, row_block_size, col_block_size, x + row_block_pos, y + col_block_pos - num_cols_e()); } @@ -248,9 +248,8 @@ void PartitionedMatrixView::UpdateBlockDiagonalEtE( block_diagonal->block_structure(); block_diagonal->SetZero(); - + const double* values = matrix_.values(); for (int r = 0; r < num_row_blocks_e_ ; ++r) { - const double* row_values = matrix_.RowBlockValues(r); const Cell& cell = bs->rows[r].cells[0]; const int row_block_size = bs->rows[r].block.size; const int block_id = cell.block_id; @@ -260,8 +259,8 @@ void PartitionedMatrixView::UpdateBlockDiagonalEtE( MatrixTransposeMatrixMultiply <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cell.position, row_block_size, col_block_size, - row_values + cell.position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, block_diagonal->mutable_values() + cell_position, 0, 0, col_block_size, col_block_size); } @@ -279,10 +278,10 @@ void PartitionedMatrixView::UpdateBlockDiagonalFtF( block_diagonal->block_structure(); block_diagonal->SetZero(); + const double* values = matrix_.values(); for (int r = 0; r < bs->rows.size(); ++r) { const int row_block_size = bs->rows[r].block.size; const vector<Cell>& cells = bs->rows[r].cells; - const double* row_values = matrix_.RowBlockValues(r); for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { const int col_block_id = cells[c].block_id; const int col_block_size = bs->cols[col_block_id].size; @@ -292,8 +291,8 @@ void PartitionedMatrixView::UpdateBlockDiagonalFtF( MatrixTransposeMatrixMultiply <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cells[c].position, row_block_size, col_block_size, - row_values + cells[c].position, row_block_size, col_block_size, + values + cells[c].position, row_block_size, col_block_size, + values + cells[c].position, row_block_size, col_block_size, block_diagonal->mutable_values() + cell_position, 0, 0, col_block_size, col_block_size); } diff --git a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h index cfe4de5b436..ebfbe403189 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h +++ b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h @@ -60,7 +60,7 @@ class PartitionedMatrixView { public: // matrix = [E F], where the matrix E contains the first // num_col_blocks_a column blocks. - PartitionedMatrixView(const BlockSparseMatrixBase& matrix, + PartitionedMatrixView(const BlockSparseMatrix& matrix, int num_col_blocks_a); ~PartitionedMatrixView(); @@ -107,7 +107,7 @@ class PartitionedMatrixView { BlockSparseMatrix* CreateBlockDiagonalMatrixLayout(int start_col_block, int end_col_block) const; - const BlockSparseMatrixBase& matrix_; + const BlockSparseMatrix& matrix_; int num_row_blocks_e_; int num_col_blocks_e_; int num_col_blocks_f_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc index 05e539f9fb1..505a47d3d61 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc @@ -45,8 +45,8 @@ SparseMatrixPreconditionerWrapper::SparseMatrixPreconditionerWrapper( SparseMatrixPreconditionerWrapper::~SparseMatrixPreconditionerWrapper() { } -bool SparseMatrixPreconditionerWrapper::Update(const BlockSparseMatrixBase& A, - const double* D) { +bool SparseMatrixPreconditionerWrapper::UpdateImpl(const SparseMatrix& A, + const double* D) { return true; } diff --git a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h index d7c88293687..af64e3c9a44 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h +++ b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h @@ -32,13 +32,15 @@ #define CERES_INTERNAL_PRECONDITIONER_H_ #include <vector> +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" #include "ceres/linear_operator.h" #include "ceres/sparse_matrix.h" namespace ceres { namespace internal { -class BlockSparseMatrixBase; +class BlockSparseMatrix; class SparseMatrix; class Preconditioner : public LinearOperator { @@ -46,7 +48,7 @@ class Preconditioner : public LinearOperator { struct Options { Options() : type(JACOBI), - sparse_linear_algebra_library(SUITE_SPARSE), + sparse_linear_algebra_library_type(SUITE_SPARSE), num_threads(1), row_block_size(Eigen::Dynamic), e_block_size(Eigen::Dynamic), @@ -55,7 +57,7 @@ class Preconditioner : public LinearOperator { PreconditionerType type; - SparseLinearAlgebraLibraryType sparse_linear_algebra_library; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; // If possible, how many threads the preconditioner can use. int num_threads; @@ -105,7 +107,7 @@ class Preconditioner : public LinearOperator { // // D can be NULL, in which case its interpreted as a diagonal matrix // of size zero. - virtual bool Update(const BlockSparseMatrixBase& A, const double* D) = 0; + virtual bool Update(const LinearOperator& A, const double* D) = 0; // LinearOperator interface. Since the operator is symmetric, // LeftMultiply and num_cols are just calls to RightMultiply and @@ -122,19 +124,40 @@ class Preconditioner : public LinearOperator { } }; +// This templated subclass of Preconditioner serves as a base class for +// other preconditioners that depend on the particular matrix layout of +// the underlying linear operator. +template <typename MatrixType> +class TypedPreconditioner : public Preconditioner { + public: + virtual ~TypedPreconditioner() {} + virtual bool Update(const LinearOperator& A, const double* D) { + return UpdateImpl(*down_cast<const MatrixType*>(&A), D); + } + + private: + virtual bool UpdateImpl(const MatrixType& A, const double* D) = 0; +}; + +// Preconditioners that depend on acccess to the low level structure +// of a SparseMatrix. +typedef TypedPreconditioner<SparseMatrix> SparseMatrixPreconditioner; // NOLINT +typedef TypedPreconditioner<BlockSparseMatrix> BlockSparseMatrixPreconditioner; // NOLINT +typedef TypedPreconditioner<CompressedRowSparseMatrix> CompressedRowSparseMatrixPreconditioner; // NOLINT + // Wrap a SparseMatrix object as a preconditioner. -class SparseMatrixPreconditionerWrapper : public Preconditioner { +class SparseMatrixPreconditionerWrapper : public SparseMatrixPreconditioner { public: // Wrapper does NOT take ownership of the matrix pointer. explicit SparseMatrixPreconditionerWrapper(const SparseMatrix* matrix); virtual ~SparseMatrixPreconditionerWrapper(); // Preconditioner interface - virtual bool Update(const BlockSparseMatrixBase& A, const double* D); virtual void RightMultiply(const double* x, double* y) const; virtual int num_rows() const; private: + virtual bool UpdateImpl(const SparseMatrix& A, const double* D); const SparseMatrix* matrix_; }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem.cc b/extern/libmv/third_party/ceres/internal/ceres/problem.cc index b483932b2c1..403e96a3ade 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/problem.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/problem.cc @@ -206,11 +206,11 @@ int Problem::NumResiduals() const { return problem_impl_->NumResiduals(); } -int Problem::ParameterBlockSize(double* parameter_block) const { +int Problem::ParameterBlockSize(const double* parameter_block) const { return problem_impl_->ParameterBlockSize(parameter_block); }; -int Problem::ParameterBlockLocalSize(double* parameter_block) const { +int Problem::ParameterBlockLocalSize(const double* parameter_block) const { return problem_impl_->ParameterBlockLocalSize(parameter_block); }; 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 34c37857538..830270269c3 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc @@ -711,13 +711,14 @@ int ProblemImpl::NumResiduals() const { return program_->NumResiduals(); } -int ProblemImpl::ParameterBlockSize(double* parameter_block) const { - return FindParameterBlockOrDie(parameter_block_map_, parameter_block)->Size(); +int ProblemImpl::ParameterBlockSize(const double* parameter_block) const { + return FindParameterBlockOrDie(parameter_block_map_, + const_cast<double*>(parameter_block))->Size(); }; -int ProblemImpl::ParameterBlockLocalSize(double* parameter_block) const { - return FindParameterBlockOrDie(parameter_block_map_, - parameter_block)->LocalSize(); +int ProblemImpl::ParameterBlockLocalSize(const double* parameter_block) const { + return FindParameterBlockOrDie( + parameter_block_map_, const_cast<double*>(parameter_block))->LocalSize(); }; void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const { 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 2609389645a..ace27f56bb1 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h @@ -139,8 +139,8 @@ class ProblemImpl { int NumResidualBlocks() const; int NumResiduals() const; - int ParameterBlockSize(double* parameter_block) const; - int ParameterBlockLocalSize(double* parameter_block) const; + int ParameterBlockSize(const double* parameter_block) const; + int ParameterBlockLocalSize(const double* parameter_block) const; void GetParameterBlocks(vector<double*>* parameter_blocks) const; const Program& program() const { return *program_; } 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 de56ac25ff6..8aa2a3977c4 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h +++ b/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h @@ -84,6 +84,7 @@ #endif #include <map> +#include <string> #include <vector> #include "ceres/execution_summary.h" #include "ceres/internal/eigen.h" @@ -91,6 +92,7 @@ #include "ceres/parameter_block.h" #include "ceres/program.h" #include "ceres/residual_block.h" +#include "ceres/small_blas.h" namespace ceres { namespace internal { @@ -230,14 +232,13 @@ class ProgramEvaluator : public Evaluator { 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; + + MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + block_jacobians[j], + num_residuals, + parameter_block->LocalSize(), + block_residuals, + scratch->gradient.get() + parameter_block->delta_offset()); } } } 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 649f3f714c2..621082ac0ea 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc @@ -34,8 +34,6 @@ #include <algorithm> #include <cstddef> #include <vector> - -#include "ceres/blas.h" #include "ceres/corrector.h" #include "ceres/parameter_block.h" #include "ceres/residual_block_utils.h" @@ -44,6 +42,7 @@ #include "ceres/internal/fixed_array.h" #include "ceres/local_parameterization.h" #include "ceres/loss_function.h" +#include "ceres/small_blas.h" using Eigen::Dynamic; 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 8afb1215015..b192aa1172b 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 @@ -33,20 +33,18 @@ #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" #include "ceres/block_random_access_sparse_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" +#include "ceres/cxsparse.h" #include "ceres/detect_structure.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" #include "ceres/internal/scoped_ptr.h" +#include "ceres/lapack.h" #include "ceres/linear_solver.h" #include "ceres/schur_complement_solver.h" #include "ceres/suitesparse.h" @@ -58,7 +56,7 @@ namespace ceres { namespace internal { LinearSolver::Summary SchurComplementSolver::SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { @@ -130,29 +128,31 @@ bool DenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { return true; } - // TODO(sameeragarwal): Add proper error handling; this completely ignores - // the quality of the solution to the solve. - VectorRef(solution, num_rows) = - ConstMatrixRef(m->values(), num_rows, num_rows) - .selfadjointView<Eigen::Upper>() - .ldlt() - .solve(ConstVectorRef(rhs(), num_rows)); + if (options().dense_linear_algebra_library_type == EIGEN) { + // TODO(sameeragarwal): Add proper error handling; this completely ignores + // the quality of the solution to the solve. + VectorRef(solution, num_rows) = + ConstMatrixRef(m->values(), num_rows, num_rows) + .selfadjointView<Eigen::Upper>() + .llt() + .solve(ConstVectorRef(rhs(), num_rows)); + return true; + } - return true; + VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows); + const int info = LAPACK::SolveInPlaceUsingCholesky(num_rows, + m->values(), + solution); + return (info == 0); } #if !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARE) SparseSchurComplementSolver::SparseSchurComplementSolver( const LinearSolver::Options& options) - : SchurComplementSolver(options) { -#ifndef CERES_NO_SUITESPARSE - factor_ = NULL; -#endif // CERES_NO_SUITESPARSE - -#ifndef CERES_NO_CXSPARSE - cxsparse_factor_ = NULL; -#endif // CERES_NO_CXSPARSE + : SchurComplementSolver(options), + factor_(NULL), + cxsparse_factor_(NULL) { } SparseSchurComplementSolver::~SparseSchurComplementSolver() { @@ -243,18 +243,18 @@ void SparseSchurComplementSolver::InitStorage( } bool SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { - switch (options().sparse_linear_algebra_library) { + switch (options().sparse_linear_algebra_library_type) { 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; + << options().sparse_linear_algebra_library_type; } LOG(FATAL) << "Unknown sparse linear algebra library : " - << options().sparse_linear_algebra_library; + << options().sparse_linear_algebra_library_type; return false; } @@ -276,26 +276,42 @@ bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( return true; } - cholmod_sparse* cholmod_lhs = ss_.CreateSparseMatrix(tsm); - // The matrix is symmetric, and the upper triangular part of the - // matrix contains the values. - cholmod_lhs->stype = 1; + cholmod_sparse* cholmod_lhs = NULL; + if (options().use_postordering) { + // If we are going to do a full symbolic analysis of the schur + // complement matrix from scratch and not rely on the + // pre-ordering, then the fastest path in cholmod_factorize is the + // one corresponding to upper triangular matrices. - cholmod_dense* cholmod_rhs = - ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows); + // Create a upper triangular symmetric matrix. + cholmod_lhs = ss_.CreateSparseMatrix(tsm); + cholmod_lhs->stype = 1; - // Symbolic factorization is computed if we don't already have one handy. - if (factor_ == NULL) { - factor_ = ss_.BlockAnalyzeCholesky(cholmod_lhs, blocks_, blocks_); + if (factor_ == NULL) { + factor_ = ss_.BlockAnalyzeCholesky(cholmod_lhs, blocks_, blocks_); + } + } else { + // If we are going to use the natural ordering (i.e. rely on the + // pre-ordering computed by solver_impl.cc), then the fastest + // path in cholmod_factorize is the one corresponding to lower + // triangular matrices. + + // Create a upper triangular symmetric matrix. + cholmod_lhs = ss_.CreateSparseMatrixTranspose(tsm); + cholmod_lhs->stype = -1; + + if (factor_ == NULL) { + factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(cholmod_lhs); + } } + cholmod_dense* cholmod_rhs = + ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows); cholmod_dense* cholmod_solution = ss_.SolveCholesky(cholmod_lhs, factor_, cholmod_rhs); ss_.Free(cholmod_lhs); - cholmod_lhs = NULL; ss_.Free(cholmod_rhs); - cholmod_rhs = NULL; if (cholmod_solution == NULL) { LOG(WARNING) << "CHOLMOD solve failed."; @@ -339,7 +355,8 @@ bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse( // Compute symbolic factorization if not available. if (cxsparse_factor_ == NULL) { - cxsparse_factor_ = CHECK_NOTNULL(cxsparse_.AnalyzeCholesky(lhs)); + cxsparse_factor_ = + CHECK_NOTNULL(cxsparse_.BlockAnalyzeCholesky(lhs, blocks_, blocks_)); } // Solve the linear system. 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 7c8d2e7ce38..b5a1c74ab1a 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 @@ -48,7 +48,7 @@ namespace ceres { namespace internal { -class BlockSparseMatrixBase; +class BlockSparseMatrix; // Base class for Schur complement based linear least squares // solvers. It assumes that the input linear system Ax = b can be @@ -100,7 +100,7 @@ class BlockSparseMatrixBase; // set to DENSE_SCHUR and SPARSE_SCHUR // respectively. LinearSolver::Options::elimination_groups[0] should be // at least 1. -class SchurComplementSolver : public BlockSparseMatrixBaseSolver { +class SchurComplementSolver : public BlockSparseMatrixSolver { public: explicit SchurComplementSolver(const LinearSolver::Options& options) : options_(options) { @@ -111,7 +111,7 @@ class SchurComplementSolver : public BlockSparseMatrixBaseSolver { // LinearSolver methods virtual ~SchurComplementSolver() {} virtual LinearSolver::Summary SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x); @@ -167,18 +167,14 @@ class SparseSchurComplementSolver : public SchurComplementSolver { // 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 in subsequent calls. cholmod_factor* factor_; -#endif // CERES_NO_SUITESPARSE -#ifndef CERES_NO_CXSPARSE CXSparse cxsparse_; // Cached factorization cs_dis* cxsparse_factor_; -#endif // CERES_NO_CXSPARSE CERES_DISALLOW_COPY_AND_ASSIGN(SparseSchurComplementSolver); }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h index f2c247a5adb..8fe8b9c88b7 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h @@ -170,7 +170,7 @@ class SchurEliminatorBase { // also the caller's responsibilty to ensure that the // CompressedRowBlockStructure object passed to this method is the // same one (or is equivalent to) the one associated with the - // BlockSparseMatrixBase objects below. + // BlockSparseMatrix objects below. virtual void Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) = 0; @@ -185,7 +185,7 @@ class SchurEliminatorBase { // // Since the Schur complement is a symmetric matrix, only the upper // triangular part of the Schur complement is computed. - virtual void Eliminate(const BlockSparseMatrixBase* A, + virtual void Eliminate(const BlockSparseMatrix* A, const double* b, const double* D, BlockRandomAccessMatrix* lhs, @@ -194,7 +194,7 @@ class SchurEliminatorBase { // Given values for the variables z in the F block of A, solve for // the optimal values of the variables y corresponding to the E // block in A. - virtual void BackSubstitute(const BlockSparseMatrixBase* A, + virtual void BackSubstitute(const BlockSparseMatrix* A, const double* b, const double* D, const double* z, @@ -226,12 +226,12 @@ class SchurEliminator : public SchurEliminatorBase { virtual ~SchurEliminator(); virtual void Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs); - virtual void Eliminate(const BlockSparseMatrixBase* A, + virtual void Eliminate(const BlockSparseMatrix* A, const double* b, const double* D, BlockRandomAccessMatrix* lhs, double* rhs); - virtual void BackSubstitute(const BlockSparseMatrixBase* A, + virtual void BackSubstitute(const BlockSparseMatrix* A, const double* b, const double* D, const double* z, @@ -273,7 +273,7 @@ class SchurEliminator : public SchurEliminatorBase { void ChunkDiagonalBlockAndGradient( const Chunk& chunk, - const BlockSparseMatrixBase* A, + const BlockSparseMatrix* A, const double* b, int row_block_counter, typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* eet, @@ -282,7 +282,7 @@ class SchurEliminator : public SchurEliminatorBase { BlockRandomAccessMatrix* lhs); void UpdateRhs(const Chunk& chunk, - const BlockSparseMatrixBase* A, + const BlockSparseMatrix* A, const double* b, int row_block_counter, const double* inverse_ete_g, @@ -293,18 +293,18 @@ class SchurEliminator : public SchurEliminatorBase { const double* buffer, const BufferLayoutType& buffer_layout, BlockRandomAccessMatrix* lhs); - void EBlockRowOuterProduct(const BlockSparseMatrixBase* A, + void EBlockRowOuterProduct(const BlockSparseMatrix* A, int row_block_index, BlockRandomAccessMatrix* lhs); - void NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, + void NoEBlockRowsUpdate(const BlockSparseMatrix* A, const double* b, int row_block_counter, BlockRandomAccessMatrix* lhs, double* rhs); - void NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, + void NoEBlockRowOuterProduct(const BlockSparseMatrix* A, int row_block_index, BlockRandomAccessMatrix* lhs); 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 835f879caf6..c09b7fb3a77 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 @@ -51,8 +51,6 @@ #include <algorithm> #include <map> - -#include "ceres/blas.h" #include "ceres/block_random_access_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" @@ -61,6 +59,7 @@ #include "ceres/internal/scoped_ptr.h" #include "ceres/map_util.h" #include "ceres/schur_eliminator.h" +#include "ceres/small_blas.h" #include "ceres/stl_util.h" #include "Eigen/Dense" #include "glog/logging.h" @@ -168,7 +167,7 @@ Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) { template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -Eliminate(const BlockSparseMatrixBase* A, +Eliminate(const BlockSparseMatrix* A, const double* b, const double* D, BlockRandomAccessMatrix* lhs, @@ -299,7 +298,7 @@ Eliminate(const BlockSparseMatrixBase* A, template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -BackSubstitute(const BlockSparseMatrixBase* A, +BackSubstitute(const BlockSparseMatrix* A, const double* b, const double* D, const double* z, @@ -324,9 +323,9 @@ BackSubstitute(const BlockSparseMatrixBase* A, ete.setZero(); } + const double* values = A->values(); for (int j = 0; j < chunk.size; ++j) { const CompressedRow& row = bs->rows[chunk.start + j]; - const double* row_values = A->RowBlockValues(chunk.start + j); const Cell& e_cell = row.cells.front(); DCHECK_EQ(e_block_id, e_cell.block_id); @@ -342,20 +341,20 @@ BackSubstitute(const BlockSparseMatrixBase* A, const int r_block = f_block_id - num_eliminate_blocks_; MatrixVectorMultiply<kRowBlockSize, kFBlockSize, -1>( - row_values + row.cells[c].position, row.block.size, f_block_size, + values + row.cells[c].position, row.block.size, f_block_size, z + lhs_row_layout_[r_block], sj.get()); } MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>( - row_values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, sj.get(), y_ptr); MatrixTransposeMatrixMultiply <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>( - row_values + e_cell.position, row.block.size, e_block_size, - row_values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, ete.data(), 0, 0, e_block_size, e_block_size); } @@ -370,7 +369,7 @@ template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: UpdateRhs(const Chunk& chunk, - const BlockSparseMatrixBase* A, + const BlockSparseMatrix* A, const double* b, int row_block_counter, const double* inverse_ete_g, @@ -380,9 +379,9 @@ UpdateRhs(const Chunk& chunk, const int e_block_size = bs->cols[e_block_id].size; int b_pos = bs->rows[row_block_counter].block.position; + const double* values = A->values(); for (int j = 0; j < chunk.size; ++j) { const CompressedRow& row = bs->rows[row_block_counter + j]; - const double *row_values = A->RowBlockValues(row_block_counter + j); const Cell& e_cell = row.cells.front(); typename EigenTypes<kRowBlockSize>::Vector sj = @@ -390,7 +389,7 @@ UpdateRhs(const Chunk& chunk, (b + b_pos, row.block.size); MatrixVectorMultiply<kRowBlockSize, kEBlockSize, -1>( - row_values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, inverse_ete_g, sj.data()); for (int c = 1; c < row.cells.size(); ++c) { @@ -399,7 +398,7 @@ UpdateRhs(const Chunk& chunk, const int block = block_id - num_eliminate_blocks_; CeresMutexLock l(rhs_locks_[block]); MatrixTransposeVectorMultiply<kRowBlockSize, kFBlockSize, 1>( - row_values + row.cells[c].position, + values + row.cells[c].position, row.block.size, block_size, sj.data(), rhs + lhs_row_layout_[block]); } @@ -431,7 +430,7 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: ChunkDiagonalBlockAndGradient( const Chunk& chunk, - const BlockSparseMatrixBase* A, + const BlockSparseMatrix* A, const double* b, int row_block_counter, typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* ete, @@ -447,9 +446,9 @@ ChunkDiagonalBlockAndGradient( // contribution of its F blocks to the Schur complement, the // contribution of its E block to the matrix EE' (ete), and the // corresponding block in the gradient vector. + const double* values = A->values(); for (int j = 0; j < chunk.size; ++j) { const CompressedRow& row = bs->rows[row_block_counter + j]; - const double *row_values = A->RowBlockValues(row_block_counter + j); if (row.cells.size() > 1) { EBlockRowOuterProduct(A, row_block_counter + j, lhs); @@ -459,13 +458,13 @@ ChunkDiagonalBlockAndGradient( const Cell& e_cell = row.cells.front(); MatrixTransposeMatrixMultiply <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>( - row_values + e_cell.position, row.block.size, e_block_size, - row_values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, ete->data(), 0, 0, e_block_size, e_block_size); // g += E_i' b_i MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>( - row_values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, b + b_pos, g); @@ -479,8 +478,8 @@ ChunkDiagonalBlockAndGradient( buffer + FindOrDie(chunk.buffer_layout, f_block_id); MatrixTransposeMatrixMultiply <kRowBlockSize, kEBlockSize, kRowBlockSize, kFBlockSize, 1>( - row_values + e_cell.position, row.block.size, e_block_size, - row_values + row.cells[c].position, row.block.size, f_block_size, + values + e_cell.position, row.block.size, e_block_size, + values + row.cells[c].position, row.block.size, f_block_size, buffer_ptr, 0, 0, e_block_size, f_block_size); } b_pos += row.block.size; @@ -551,21 +550,21 @@ ChunkOuterProduct(const CompressedRowBlockStructure* bs, template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, +NoEBlockRowsUpdate(const BlockSparseMatrix* A, const double* b, int row_block_counter, BlockRandomAccessMatrix* lhs, double* rhs) { const CompressedRowBlockStructure* bs = A->block_structure(); + const double* values = A->values(); for (; row_block_counter < bs->rows.size(); ++row_block_counter) { const CompressedRow& row = bs->rows[row_block_counter]; - const double *row_values = A->RowBlockValues(row_block_counter); for (int c = 0; c < row.cells.size(); ++c) { const int block_id = row.cells[c].block_id; const int block_size = bs->cols[block_id].size; const int block = block_id - num_eliminate_blocks_; MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + row.cells[c].position, row.block.size, block_size, + values + row.cells[c].position, row.block.size, block_size, b + row.block.position, rhs + lhs_row_layout_[block]); } @@ -591,12 +590,12 @@ NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, +NoEBlockRowOuterProduct(const BlockSparseMatrix* A, int row_block_index, BlockRandomAccessMatrix* lhs) { const CompressedRowBlockStructure* bs = A->block_structure(); const CompressedRow& row = bs->rows[row_block_index]; - const double *row_values = A->RowBlockValues(row_block_index); + const double* values = A->values(); for (int i = 0; i < row.cells.size(); ++i) { const int block1 = row.cells[i].block_id - num_eliminate_blocks_; DCHECK_GE(block1, 0); @@ -612,8 +611,8 @@ NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, // symmetric outer product. MatrixTransposeMatrixMultiply <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + row.cells[i].position, row.block.size, block1_size, - row_values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[i].position, row.block.size, block1_size, cell_info->values, r, c, row_stride, col_stride); } @@ -630,8 +629,8 @@ NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, CeresMutexLock l(&cell_info->m); MatrixTransposeMatrixMultiply <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + row.cells[i].position, row.block.size, block1_size, - row_values + row.cells[j].position, row.block.size, block2_size, + values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[j].position, row.block.size, block2_size, cell_info->values, r, c, row_stride, col_stride); } } @@ -644,12 +643,12 @@ NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -EBlockRowOuterProduct(const BlockSparseMatrixBase* A, +EBlockRowOuterProduct(const BlockSparseMatrix* A, int row_block_index, BlockRandomAccessMatrix* lhs) { const CompressedRowBlockStructure* bs = A->block_structure(); const CompressedRow& row = bs->rows[row_block_index]; - const double *row_values = A->RowBlockValues(row_block_index); + const double* values = A->values(); for (int i = 1; i < row.cells.size(); ++i) { const int block1 = row.cells[i].block_id - num_eliminate_blocks_; DCHECK_GE(block1, 0); @@ -664,8 +663,8 @@ EBlockRowOuterProduct(const BlockSparseMatrixBase* A, // block += b1.transpose() * b1; MatrixTransposeMatrixMultiply <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>( - row_values + row.cells[i].position, row.block.size, block1_size, - row_values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[i].position, row.block.size, block1_size, cell_info->values, r, c, row_stride, col_stride); } @@ -683,8 +682,8 @@ EBlockRowOuterProduct(const BlockSparseMatrixBase* A, CeresMutexLock l(&cell_info->m); MatrixTransposeMatrixMultiply <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>( - row_values + row.cells[i].position, row.block.size, block1_size, - row_values + row.cells[j].position, row.block.size, block2_size, + values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[j].position, row.block.size, block2_size, cell_info->values, r, c, row_stride, col_stride); } } diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.cc index 33a666ed037..338df715c0a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.cc @@ -91,8 +91,8 @@ void SchurJacobiPreconditioner::InitEliminator( } // Update the values of the preconditioner matrix and factorize it. -bool SchurJacobiPreconditioner::Update(const BlockSparseMatrixBase& A, - const double* D) { +bool SchurJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, + const double* D) { const int num_rows = m_->num_rows(); CHECK_GT(num_rows, 0); @@ -128,7 +128,7 @@ void SchurJacobiPreconditioner::RightMultiply(const double* x, VectorRef(y, block_size) = block .selfadjointView<Eigen::Upper>() - .ldlt() + .llt() .solve(ConstVectorRef(x, block_size)); x += block_size; diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.h index 3addd73abd2..f6e7b0d37ef 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.h +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.h @@ -50,7 +50,7 @@ namespace ceres { namespace internal { class BlockRandomAccessSparseMatrix; -class BlockSparseMatrixBase; +class BlockSparseMatrix; struct CompressedRowBlockStructure; class SchurEliminatorBase; @@ -73,7 +73,7 @@ class SchurEliminatorBase; // preconditioner.Update(A, NULL); // preconditioner.RightMultiply(x, y); // -class SchurJacobiPreconditioner : public Preconditioner { +class SchurJacobiPreconditioner : public BlockSparseMatrixPreconditioner { public: // Initialize the symbolic structure of the preconditioner. bs is // the block structure of the linear system to be solved. It is used @@ -86,12 +86,12 @@ class SchurJacobiPreconditioner : public Preconditioner { virtual ~SchurJacobiPreconditioner(); // Preconditioner interface. - virtual bool Update(const BlockSparseMatrixBase& A, const double* D); virtual void RightMultiply(const double* x, double* y) const; virtual int num_rows() const; private: void InitEliminator(const CompressedRowBlockStructure& bs); + virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); Preconditioner::Options options_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/small_blas.h b/extern/libmv/third_party/ceres/internal/ceres/small_blas.h new file mode 100644 index 00000000000..e14e664b7fa --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/small_blas.h @@ -0,0 +1,406 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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) +// +// Simple blas functions for use in the Schur Eliminator. These are +// fairly basic implementations which already yield a significant +// speedup in the eliminator performance. + +#ifndef CERES_INTERNAL_SMALL_BLAS_H_ +#define CERES_INTERNAL_SMALL_BLAS_H_ + +#include "ceres/internal/eigen.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Remove the ".noalias()" annotation from the matrix matrix +// mutliplies 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() -= A * B; +// +// gets compiled to +// +// block.noalias() += A * B; +// +// 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. +#ifdef CERES_WORK_AROUND_ANDROID_NDK_COMPILER_BUG +#define CERES_MAYBE_NOALIAS +#else +#define CERES_MAYBE_NOALIAS .noalias() +#endif + +// The following three macros are used to share code and reduce +// template junk across the various GEMM variants. +#define CERES_GEMM_BEGIN(name) \ + template<int kRowA, int kColA, int kRowB, int kColB, int kOperation> \ + inline void name(const double* A, \ + const int num_row_a, \ + const int num_col_a, \ + const double* B, \ + const int num_row_b, \ + const int num_col_b, \ + double* C, \ + const int start_row_c, \ + const int start_col_c, \ + const int row_stride_c, \ + const int col_stride_c) + +#define CERES_GEMM_NAIVE_HEADER \ + DCHECK_GT(num_row_a, 0); \ + DCHECK_GT(num_col_a, 0); \ + DCHECK_GT(num_row_b, 0); \ + DCHECK_GT(num_col_b, 0); \ + DCHECK_GE(start_row_c, 0); \ + DCHECK_GE(start_col_c, 0); \ + DCHECK_GT(row_stride_c, 0); \ + DCHECK_GT(col_stride_c, 0); \ + DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); \ + DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); \ + DCHECK((kRowB == Eigen::Dynamic) || (kRowB == num_row_b)); \ + DCHECK((kColB == Eigen::Dynamic) || (kColB == num_col_b)); \ + const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); \ + const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); \ + const int NUM_ROW_B = (kColB != Eigen::Dynamic ? kRowB : num_row_b); \ + const int NUM_COL_B = (kColB != Eigen::Dynamic ? kColB : num_col_b); + +#define CERES_GEMM_EIGEN_HEADER \ + const typename EigenTypes<kRowA, kColA>::ConstMatrixRef \ + Aref(A, num_row_a, num_col_a); \ + const typename EigenTypes<kRowB, kColB>::ConstMatrixRef \ + Bref(B, num_row_b, num_col_b); \ + MatrixRef Cref(C, row_stride_c, col_stride_c); \ + +#define CERES_CALL_GEMM(name) \ + name<kRowA, kColA, kRowB, kColB, kOperation>( \ + A, num_row_a, num_col_a, \ + B, num_row_b, num_col_b, \ + C, start_row_c, start_col_c, row_stride_c, col_stride_c); + + +// For the matrix-matrix functions below, there are three variants for +// each functionality. Foo, FooNaive and FooEigen. Foo is the one to +// be called by the user. FooNaive is a basic loop based +// implementation and FooEigen uses Eigen's implementation. Foo +// chooses between FooNaive and FooEigen depending on how many of the +// template arguments are fixed at compile time. Currently, FooEigen +// is called if all matrix dimensions are compile time +// constants. FooNaive is called otherwise. This leads to the best +// performance currently. +// +// The MatrixMatrixMultiply variants compute: +// +// C op A * B; +// +// The MatrixTransposeMatrixMultiply variants compute: +// +// C op A' * B +// +// where op can be +=, -=, or =. +// +// The template parameters (kRowA, kColA, kRowB, kColB) allow +// specialization of the loop at compile time. If this information is +// not available, then Eigen::Dynamic should be used as the template +// argument. +// +// kOperation = 1 -> C += A * B +// kOperation = -1 -> C -= A * B +// kOperation = 0 -> C = A * B +// +// The functions can write into matrices C which are larger than the +// matrix A * B. This is done by specifying the true size of C via +// row_stride_c and col_stride_c, and then indicating where A * B +// should be written into by start_row_c and start_col_c. +// +// Graphically if row_stride_c = 10, col_stride_c = 12, start_row_c = +// 4 and start_col_c = 5, then if A = 3x2 and B = 2x4, we get +// +// ------------ +// ------------ +// ------------ +// ------------ +// -----xxxx--- +// -----xxxx--- +// -----xxxx--- +// ------------ +// ------------ +// ------------ +// +CERES_GEMM_BEGIN(MatrixMatrixMultiplyEigen) { + CERES_GEMM_EIGEN_HEADER + Eigen::Block<MatrixRef, kRowA, kColB> + block(Cref, start_row_c, start_col_c, num_row_a, num_col_b); + + if (kOperation > 0) { + block CERES_MAYBE_NOALIAS += Aref * Bref; + } else if (kOperation < 0) { + block CERES_MAYBE_NOALIAS -= Aref * Bref; + } else { + block CERES_MAYBE_NOALIAS = Aref * Bref; + } +} + +CERES_GEMM_BEGIN(MatrixMatrixMultiplyNaive) { + CERES_GEMM_NAIVE_HEADER + DCHECK_EQ(NUM_COL_A, NUM_ROW_B); + + const int NUM_ROW_C = NUM_ROW_A; + const int NUM_COL_C = NUM_COL_B; + DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c); + DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c); + + for (int row = 0; row < NUM_ROW_C; ++row) { + for (int col = 0; col < NUM_COL_C; ++col) { + double tmp = 0.0; + for (int k = 0; k < NUM_COL_A; ++k) { + tmp += A[row * NUM_COL_A + k] * B[k * NUM_COL_B + col]; + } + + const int index = (row + start_row_c) * col_stride_c + start_col_c + col; + if (kOperation > 0) { + C[index] += tmp; + } else if (kOperation < 0) { + C[index] -= tmp; + } else { + C[index] = tmp; + } + } + } +} + +CERES_GEMM_BEGIN(MatrixMatrixMultiply) { +#ifdef CERES_NO_CUSTOM_BLAS + + CERES_CALL_GEMM(MatrixMatrixMultiplyEigen) + return; + +#else + + if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic && + kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) { + CERES_CALL_GEMM(MatrixMatrixMultiplyEigen) + } else { + CERES_CALL_GEMM(MatrixMatrixMultiplyNaive) + } + +#endif +} + +CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyEigen) { + CERES_GEMM_EIGEN_HEADER + Eigen::Block<MatrixRef, kColA, kColB> block(Cref, + start_row_c, start_col_c, + num_col_a, num_col_b); + if (kOperation > 0) { + block CERES_MAYBE_NOALIAS += Aref.transpose() * Bref; + } else if (kOperation < 0) { + block CERES_MAYBE_NOALIAS -= Aref.transpose() * Bref; + } else { + block CERES_MAYBE_NOALIAS = Aref.transpose() * Bref; + } +} + +CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyNaive) { + CERES_GEMM_NAIVE_HEADER + DCHECK_EQ(NUM_ROW_A, NUM_ROW_B); + + const int NUM_ROW_C = NUM_COL_A; + const int NUM_COL_C = NUM_COL_B; + DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c); + DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c); + + for (int row = 0; row < NUM_ROW_C; ++row) { + for (int col = 0; col < NUM_COL_C; ++col) { + double tmp = 0.0; + for (int k = 0; k < NUM_ROW_A; ++k) { + tmp += A[k * NUM_COL_A + row] * B[k * NUM_COL_B + col]; + } + + const int index = (row + start_row_c) * col_stride_c + start_col_c + col; + if (kOperation > 0) { + C[index]+= tmp; + } else if (kOperation < 0) { + C[index]-= tmp; + } else { + C[index]= tmp; + } + } + } +} + +CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiply) { +#ifdef CERES_NO_CUSTOM_BLAS + + CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen) + return; + +#else + + if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic && + kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) { + CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen) + } else { + CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyNaive) + } + +#endif +} + +// Matrix-Vector multiplication +// +// c op A * b; +// +// where op can be +=, -=, or =. +// +// The template parameters (kRowA, kColA) allow specialization of the +// loop at compile time. If this information is not available, then +// Eigen::Dynamic should be used as the template argument. +// +// kOperation = 1 -> c += A' * b +// kOperation = -1 -> c -= A' * b +// kOperation = 0 -> c = A' * b +template<int kRowA, int kColA, int kOperation> +inline void MatrixVectorMultiply(const double* A, + const int num_row_a, + const int num_col_a, + const double* b, + double* c) { +#ifdef CERES_NO_CUSTOM_BLAS + const typename EigenTypes<kRowA, kColA>::ConstMatrixRef + Aref(A, num_row_a, num_col_a); + const typename EigenTypes<kColA>::ConstVectorRef bref(b, num_col_a); + typename EigenTypes<kRowA>::VectorRef cref(c, num_row_a); + + // lazyProduct works better than .noalias() for matrix-vector + // products. + if (kOperation > 0) { + cref += Aref.lazyProduct(bref); + } else if (kOperation < 0) { + cref -= Aref.lazyProduct(bref); + } else { + cref = Aref.lazyProduct(bref); + } +#else + + DCHECK_GT(num_row_a, 0); + DCHECK_GT(num_col_a, 0); + DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); + DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); + + const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); + const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); + + for (int row = 0; row < NUM_ROW_A; ++row) { + double tmp = 0.0; + for (int col = 0; col < NUM_COL_A; ++col) { + tmp += A[row * NUM_COL_A + col] * b[col]; + } + + if (kOperation > 0) { + c[row] += tmp; + } else if (kOperation < 0) { + c[row] -= tmp; + } else { + c[row] = tmp; + } + } +#endif // CERES_NO_CUSTOM_BLAS +} + +// Similar to MatrixVectorMultiply, except that A is transposed, i.e., +// +// c op A' * b; +template<int kRowA, int kColA, int kOperation> +inline void MatrixTransposeVectorMultiply(const double* A, + const int num_row_a, + const int num_col_a, + const double* b, + double* c) { +#ifdef CERES_NO_CUSTOM_BLAS + const typename EigenTypes<kRowA, kColA>::ConstMatrixRef + Aref(A, num_row_a, num_col_a); + const typename EigenTypes<kRowA>::ConstVectorRef bref(b, num_row_a); + typename EigenTypes<kColA>::VectorRef cref(c, num_col_a); + + // lazyProduct works better than .noalias() for matrix-vector + // products. + if (kOperation > 0) { + cref += Aref.transpose().lazyProduct(bref); + } else if (kOperation < 0) { + cref -= Aref.transpose().lazyProduct(bref); + } else { + cref = Aref.transpose().lazyProduct(bref); + } +#else + + DCHECK_GT(num_row_a, 0); + DCHECK_GT(num_col_a, 0); + DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); + DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); + + const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); + const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); + + for (int row = 0; row < NUM_COL_A; ++row) { + double tmp = 0.0; + for (int col = 0; col < NUM_ROW_A; ++col) { + tmp += A[col * NUM_COL_A + row] * b[col]; + } + + if (kOperation > 0) { + c[row] += tmp; + } else if (kOperation < 0) { + c[row] -= tmp; + } else { + c[row] = tmp; + } + } +#endif // CERES_NO_CUSTOM_BLAS +} + + +#undef CERES_MAYBE_NOALIAS +#undef CERES_GEMM_BEGIN +#undef CERES_GEMM_EIGEN_HEADER +#undef CERES_GEMM_NAIVE_HEADER +#undef CERES_CALL_GEMM + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SMALL_BLAS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver.cc b/extern/libmv/third_party/ceres/internal/ceres/solver.cc index ea9ff1f488b..3b67746044c 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/solver.cc @@ -91,6 +91,7 @@ Solver::Summary::Summary() fixed_cost(-1.0), num_successful_steps(-1), num_unsuccessful_steps(-1), + num_inner_iteration_steps(-1), preprocessor_time_in_seconds(-1.0), minimizer_time_in_seconds(-1.0), postprocessor_time_in_seconds(-1.0), @@ -98,6 +99,7 @@ Solver::Summary::Summary() linear_solver_time_in_seconds(-1.0), residual_evaluation_time_in_seconds(-1.0), jacobian_evaluation_time_in_seconds(-1.0), + inner_iteration_time_in_seconds(-1.0), num_parameter_blocks(-1), num_parameters(-1), num_effective_parameters(-1), @@ -114,10 +116,12 @@ Solver::Summary::Summary() num_linear_solver_threads_used(-1), linear_solver_type_given(SPARSE_NORMAL_CHOLESKY), linear_solver_type_used(SPARSE_NORMAL_CHOLESKY), + inner_iterations_given(false), + inner_iterations_used(false), preconditioner_type(IDENTITY), trust_region_strategy_type(LEVENBERG_MARQUARDT), - inner_iterations(false), - sparse_linear_algebra_library(SUITE_SPARSE), + dense_linear_algebra_library_type(EIGEN), + sparse_linear_algebra_library_type(SUITE_SPARSE), line_search_direction_type(LBFGS), line_search_type(ARMIJO) { } @@ -149,6 +153,7 @@ string Solver::Summary::BriefReport() const { }; using internal::StringAppendF; +using internal::StringPrintf; string Solver::Summary::FullReport() const { string report = @@ -184,22 +189,30 @@ string Solver::Summary::FullReport() const { num_residuals, num_residuals_reduced); } - // TODO(sameeragarwal): Refactor this into separate functions. - if (minimizer_type == TRUST_REGION) { + // TRUST_SEARCH HEADER StringAppendF(&report, "\nMinimizer %19s\n", "TRUST_REGION"); + + if (linear_solver_type_used == DENSE_NORMAL_CHOLESKY || + linear_solver_type_used == DENSE_SCHUR || + linear_solver_type_used == DENSE_QR) { + StringAppendF(&report, "\nDense linear algebra library %15s\n", + DenseLinearAlgebraLibraryTypeToString( + dense_linear_algebra_library_type)); + } + if (linear_solver_type_used == SPARSE_NORMAL_CHOLESKY || linear_solver_type_used == SPARSE_SCHUR || (linear_solver_type_used == ITERATIVE_SCHUR && (preconditioner_type == CLUSTER_JACOBI || preconditioner_type == CLUSTER_TRIDIAGONAL))) { - StringAppendF(&report, "\nSparse Linear Algebra Library %15s\n", + StringAppendF(&report, "\nSparse linear algebra library %15s\n", SparseLinearAlgebraLibraryTypeToString( - sparse_linear_algebra_library)); + sparse_linear_algebra_library_type)); } - StringAppendF(&report, "Trust Region Strategy %19s", + StringAppendF(&report, "Trust region strategy %19s", TrustRegionStrategyTypeToString( trust_region_strategy_type)); if (trust_region_strategy_type == DOGLEG) { @@ -222,12 +235,9 @@ string Solver::Summary::FullReport() const { StringAppendF(&report, "Preconditioner %25s%25s\n", PreconditionerTypeToString(preconditioner_type), PreconditionerTypeToString(preconditioner_type)); - } else { - StringAppendF(&report, "Preconditioner %25s%25s\n", - "N/A", "N/A"); } - StringAppendF(&report, "Threads: % 25d% 25d\n", + StringAppendF(&report, "Threads % 25d% 25d\n", num_threads_given, num_threads_used); StringAppendF(&report, "Linear solver threads % 23d% 25d\n", num_linear_solver_threads_given, @@ -244,7 +254,14 @@ string Solver::Summary::FullReport() const { used.c_str()); } - if (inner_iterations) { + if (inner_iterations_given) { + StringAppendF(&report, + "Use inner iterations %20s %20s\n", + inner_iterations_given ? "True" : "False", + inner_iterations_used ? "True" : "False"); + } + + if (inner_iterations_used) { string given; StringifyOrdering(inner_iteration_ordering_given, &given); string used; @@ -254,119 +271,107 @@ string Solver::Summary::FullReport() const { given.c_str(), used.c_str()); } - - if (termination_type == DID_NOT_RUN) { - CHECK(!error.empty()) - << "Solver terminated with DID_NOT_RUN but the solver did not " - << "return a reason. This is a Ceres error. Please report this " - << "to the Ceres team"; - StringAppendF(&report, "Termination: %20s\n", - "DID_NOT_RUN"); - StringAppendF(&report, "Reason: %s\n", error.c_str()); - return report; - } - - StringAppendF(&report, "\nCost:\n"); - StringAppendF(&report, "Initial % 30e\n", initial_cost); - if (termination_type != NUMERICAL_FAILURE && - termination_type != USER_ABORT) { - StringAppendF(&report, "Final % 30e\n", final_cost); - StringAppendF(&report, "Change % 30e\n", - initial_cost - final_cost); - } - - StringAppendF(&report, "\nNumber of iterations:\n"); - StringAppendF(&report, "Successful % 20d\n", - num_successful_steps); - StringAppendF(&report, "Unsuccessful % 20d\n", - num_unsuccessful_steps); - StringAppendF(&report, "Total % 20d\n", - num_successful_steps + num_unsuccessful_steps); - - StringAppendF(&report, "\nTime (in seconds):\n"); - StringAppendF(&report, "Preprocessor %25.3f\n", - preprocessor_time_in_seconds); - StringAppendF(&report, "\n Residual Evaluations %22.3f\n", - residual_evaluation_time_in_seconds); - StringAppendF(&report, " Jacobian Evaluations %22.3f\n", - jacobian_evaluation_time_in_seconds); - StringAppendF(&report, " Linear Solver %23.3f\n", - linear_solver_time_in_seconds); - StringAppendF(&report, "Minimizer %25.3f\n\n", - minimizer_time_in_seconds); - - StringAppendF(&report, "Postprocessor %24.3f\n", - postprocessor_time_in_seconds); - - StringAppendF(&report, "Total %25.3f\n\n", - total_time_in_seconds); - - StringAppendF(&report, "Termination: %25s\n", - SolverTerminationTypeToString(termination_type)); } else { - // LINE_SEARCH + // LINE_SEARCH HEADER StringAppendF(&report, "\nMinimizer %19s\n", "LINE_SEARCH"); + + + string line_search_direction_string; if (line_search_direction_type == LBFGS) { - StringAppendF(&report, "Line search direction %19s(%d)\n", - LineSearchDirectionTypeToString(line_search_direction_type), - max_lbfgs_rank); + line_search_direction_string = StringPrintf("LBFGS (%d)", max_lbfgs_rank); + } else if (line_search_direction_type == NONLINEAR_CONJUGATE_GRADIENT) { + line_search_direction_string = + NonlinearConjugateGradientTypeToString( + nonlinear_conjugate_gradient_type); } else { - StringAppendF(&report, "Line search direction %19s\n", - LineSearchDirectionTypeToString( - line_search_direction_type)); + line_search_direction_string = + LineSearchDirectionTypeToString(line_search_direction_type); } - StringAppendF(&report, "Line search type %19s\n", - LineSearchTypeToString(line_search_type)); + StringAppendF(&report, "Line search direction %19s\n", + line_search_direction_string.c_str()); + + const string line_search_type_string = + StringPrintf("%s %s", + LineSearchInterpolationTypeToString( + line_search_interpolation_type), + LineSearchTypeToString(line_search_type)); + StringAppendF(&report, "Line search type %19s\n", + line_search_type_string.c_str()); StringAppendF(&report, "\n"); StringAppendF(&report, "%45s %21s\n", "Given", "Used"); - StringAppendF(&report, "Threads: % 25d% 25d\n", + StringAppendF(&report, "Threads % 25d% 25d\n", num_threads_given, num_threads_used); + } - if (termination_type == DID_NOT_RUN) { - CHECK(!error.empty()) - << "Solver terminated with DID_NOT_RUN but the solver did not " - << "return a reason. This is a Ceres error. Please report this " - << "to the Ceres team"; - StringAppendF(&report, "Termination: %20s\n", - "DID_NOT_RUN"); - StringAppendF(&report, "Reason: %s\n", error.c_str()); - return report; - } + if (termination_type == DID_NOT_RUN) { + CHECK(!error.empty()) + << "Solver terminated with DID_NOT_RUN but the solver did not " + << "return a reason. This is a Ceres error. Please report this " + << "to the Ceres team"; + StringAppendF(&report, "Termination: %20s\n", + "DID_NOT_RUN"); + StringAppendF(&report, "Reason: %s\n", error.c_str()); + return report; + } - StringAppendF(&report, "\nCost:\n"); - StringAppendF(&report, "Initial % 30e\n", initial_cost); - if (termination_type != NUMERICAL_FAILURE && - termination_type != USER_ABORT) { - StringAppendF(&report, "Final % 30e\n", final_cost); - StringAppendF(&report, "Change % 30e\n", - initial_cost - final_cost); - } + StringAppendF(&report, "\nCost:\n"); + StringAppendF(&report, "Initial % 30e\n", initial_cost); + if (termination_type != NUMERICAL_FAILURE && + termination_type != USER_ABORT) { + StringAppendF(&report, "Final % 30e\n", final_cost); + StringAppendF(&report, "Change % 30e\n", + initial_cost - final_cost); + } - StringAppendF(&report, "\nNumber of iterations: % 20d\n", - static_cast<int>(iterations.size() - 1)); + StringAppendF(&report, "\nMinimizer iterations % 16d\n", + num_successful_steps + num_unsuccessful_steps); - StringAppendF(&report, "\nTime (in seconds):\n"); - StringAppendF(&report, "Preprocessor %25.3f\n", - preprocessor_time_in_seconds); - StringAppendF(&report, "\n Residual Evaluations %22.3f\n", - residual_evaluation_time_in_seconds); - StringAppendF(&report, " Jacobian Evaluations %22.3f\n", - jacobian_evaluation_time_in_seconds); - StringAppendF(&report, "Minimizer %25.3f\n\n", - minimizer_time_in_seconds); + // Successful/Unsuccessful steps only matter in the case of the + // trust region solver. Line search terminates when it encounters + // the first unsuccessful step. + if (minimizer_type == TRUST_REGION) { + StringAppendF(&report, "Successful steps % 14d\n", + num_successful_steps); + StringAppendF(&report, "Unsuccessful steps % 14d\n", + num_unsuccessful_steps); + } + if (inner_iterations_used) { + StringAppendF(&report, "Steps with inner iterations % 14d\n", + num_inner_iteration_steps); + } - StringAppendF(&report, "Postprocessor %24.3f\n", - postprocessor_time_in_seconds); + StringAppendF(&report, "\nTime (in seconds):\n"); + StringAppendF(&report, "Preprocessor %25.3f\n", + preprocessor_time_in_seconds); - StringAppendF(&report, "Total %25.3f\n\n", - total_time_in_seconds); + StringAppendF(&report, "\n Residual evaluation %23.3f\n", + residual_evaluation_time_in_seconds); + StringAppendF(&report, " Jacobian evaluation %23.3f\n", + jacobian_evaluation_time_in_seconds); - StringAppendF(&report, "Termination: %25s\n", - SolverTerminationTypeToString(termination_type)); + if (minimizer_type == TRUST_REGION) { + StringAppendF(&report, " Linear solver %23.3f\n", + linear_solver_time_in_seconds); + } + + if (inner_iterations_used) { + StringAppendF(&report, " Inner iterations %23.3f\n", + inner_iteration_time_in_seconds); } + StringAppendF(&report, "Minimizer %25.3f\n\n", + minimizer_time_in_seconds); + + StringAppendF(&report, "Postprocessor %24.3f\n", + postprocessor_time_in_seconds); + + StringAppendF(&report, "Total %25.3f\n\n", + total_time_in_seconds); + + StringAppendF(&report, "Termination: %25s\n", + SolverTerminationTypeToString(termination_type)); return report; }; 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 43c0be6180d..83faa0510c0 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc @@ -33,7 +33,9 @@ #include <cstdio> #include <iostream> // NOLINT #include <numeric> +#include <string> #include "ceres/coordinate_descent_minimizer.h" +#include "ceres/cxsparse.h" #include "ceres/evaluator.h" #include "ceres/gradient_checking_cost_function.h" #include "ceres/iteration_callback.h" @@ -253,8 +255,8 @@ void SolverImpl::TrustRegionMinimize( 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.min_lm_diagonal = options.min_lm_diagonal; + trust_region_strategy_options.max_lm_diagonal = options.max_lm_diagonal; trust_region_strategy_options.trust_region_strategy_type = options.trust_region_strategy_type; trust_region_strategy_options.dogleg_type = options.dogleg_type; @@ -315,6 +317,16 @@ void SolverImpl::LineSearchMinimize( void SolverImpl::Solve(const Solver::Options& options, ProblemImpl* problem_impl, Solver::Summary* summary) { + VLOG(2) << "Initial problem: " + << problem_impl->NumParameterBlocks() + << " parameter blocks, " + << problem_impl->NumParameters() + << " parameters, " + << problem_impl->NumResidualBlocks() + << " residual blocks, " + << problem_impl->NumResiduals() + << " residuals."; + if (options.minimizer_type == TRUST_REGION) { TrustRegionSolve(options, problem_impl, summary); } else { @@ -389,9 +401,13 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, summary->num_threads_given = original_options.num_threads; summary->num_threads_used = options.num_threads; - if (options.lsqp_iterations_to_dump.size() > 0) { - LOG(WARNING) << "Dumping linear least squares problems to disk is" - " currently broken. Ignoring Solver::Options::lsqp_iterations_to_dump"; + if (options.trust_region_minimizer_iterations_to_dump.size() > 0 && + options.trust_region_problem_dump_format_type != CONSOLE && + options.trust_region_problem_dump_directory.empty()) { + summary->error = + "Solver::Options::trust_region_problem_dump_directory is empty."; + LOG(ERROR) << summary->error; + return; } event_logger.AddEvent("Init"); @@ -500,8 +516,10 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, original_options.num_linear_solver_threads; summary->num_linear_solver_threads_used = options.num_linear_solver_threads; - summary->sparse_linear_algebra_library = - options.sparse_linear_algebra_library; + summary->dense_linear_algebra_library_type = + options.dense_linear_algebra_library_type; + summary->sparse_linear_algebra_library_type = + options.sparse_linear_algebra_library_type; summary->trust_region_strategy_type = options.trust_region_strategy_type; summary->dogleg_type = options.dogleg_type; @@ -534,8 +552,7 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, } } } - - event_logger.AddEvent("CreateIIM"); + event_logger.AddEvent("CreateInnerIterationMinimizer"); // The optimizer works on contiguous parameter vectors; allocate some. Vector parameters(reduced_program->NumParameters()); @@ -619,10 +636,98 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, original_options.line_search_direction_type; summary->max_lbfgs_rank = original_options.max_lbfgs_rank; summary->line_search_type = original_options.line_search_type; - 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->line_search_interpolation_type = + original_options.line_search_interpolation_type; + summary->nonlinear_conjugate_gradient_type = + original_options.nonlinear_conjugate_gradient_type; + + summary->num_parameter_blocks = original_program->NumParameterBlocks(); + summary->num_parameters = original_program->NumParameters(); + summary->num_residual_blocks = original_program->NumResidualBlocks(); + summary->num_residuals = original_program->NumResiduals(); + summary->num_effective_parameters = + original_program->NumEffectiveParameters(); + + // Validate values for configuration parameters supplied by user. + if ((original_options.line_search_direction_type == ceres::BFGS || + original_options.line_search_direction_type == ceres::LBFGS) && + original_options.line_search_type != ceres::WOLFE) { + summary->error = + string("Invalid configuration: require line_search_type == " + "ceres::WOLFE when using (L)BFGS to ensure that underlying " + "assumptions are guaranteed to be satisfied."); + LOG(ERROR) << summary->error; + return; + } + if (original_options.max_lbfgs_rank <= 0) { + summary->error = + string("Invalid configuration: require max_lbfgs_rank > 0"); + LOG(ERROR) << summary->error; + return; + } + if (original_options.min_line_search_step_size <= 0.0) { + summary->error = "Invalid configuration: min_line_search_step_size <= 0.0."; + LOG(ERROR) << summary->error; + return; + } + if (original_options.line_search_sufficient_function_decrease <= 0.0) { + summary->error = + string("Invalid configuration: require ") + + string("line_search_sufficient_function_decrease <= 0.0."); + LOG(ERROR) << summary->error; + return; + } + if (original_options.max_line_search_step_contraction <= 0.0 || + original_options.max_line_search_step_contraction >= 1.0) { + summary->error = string("Invalid configuration: require ") + + string("0.0 < max_line_search_step_contraction < 1.0."); + LOG(ERROR) << summary->error; + return; + } + if (original_options.min_line_search_step_contraction <= + original_options.max_line_search_step_contraction || + original_options.min_line_search_step_contraction > 1.0) { + summary->error = string("Invalid configuration: require ") + + string("max_line_search_step_contraction < ") + + string("min_line_search_step_contraction <= 1.0."); + LOG(ERROR) << summary->error; + return; + } + // Warn user if they have requested BISECTION interpolation, but constraints + // on max/min step size change during line search prevent bisection scaling + // from occurring. Warn only, as this is likely a user mistake, but one which + // does not prevent us from continuing. + LOG_IF(WARNING, + (original_options.line_search_interpolation_type == ceres::BISECTION && + (original_options.max_line_search_step_contraction > 0.5 || + original_options.min_line_search_step_contraction < 0.5))) + << "Line search interpolation type is BISECTION, but specified " + << "max_line_search_step_contraction: " + << original_options.max_line_search_step_contraction << ", and " + << "min_line_search_step_contraction: " + << original_options.min_line_search_step_contraction + << ", prevent bisection (0.5) scaling, continuing with solve regardless."; + if (original_options.max_num_line_search_step_size_iterations <= 0) { + summary->error = string("Invalid configuration: require ") + + string("max_num_line_search_step_size_iterations > 0."); + LOG(ERROR) << summary->error; + return; + } + if (original_options.line_search_sufficient_curvature_decrease <= + original_options.line_search_sufficient_function_decrease || + original_options.line_search_sufficient_curvature_decrease > 1.0) { + summary->error = string("Invalid configuration: require ") + + string("line_search_sufficient_function_decrease < ") + + string("line_search_sufficient_curvature_decrease < 1.0."); + LOG(ERROR) << summary->error; + return; + } + if (original_options.max_line_search_step_expansion <= 1.0) { + summary->error = string("Invalid configuration: require ") + + string("max_line_search_step_expansion > 1.0."); + LOG(ERROR) << summary->error; + return; + } // Empty programs are usually a user error. if (summary->num_parameter_blocks == 0) { @@ -712,6 +817,8 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, summary->num_parameter_blocks_reduced = reduced_program->NumParameterBlocks(); summary->num_parameters_reduced = reduced_program->NumParameters(); summary->num_residual_blocks_reduced = reduced_program->NumResidualBlocks(); + summary->num_effective_parameters_reduced = + reduced_program->NumEffectiveParameters(); summary->num_residuals_reduced = reduced_program->NumResiduals(); if (summary->num_parameter_blocks_reduced == 0) { @@ -972,6 +1079,16 @@ Program* SolverImpl::CreateReducedProgram(Solver::Options* options, return NULL; } + VLOG(2) << "Reduced problem: " + << transformed_program->NumParameterBlocks() + << " parameter blocks, " + << transformed_program->NumParameters() + << " parameters, " + << transformed_program->NumResidualBlocks() + << " residual blocks, " + << transformed_program->NumResiduals() + << " residuals."; + if (transformed_program->NumParameterBlocks() == 0) { LOG(WARNING) << "No varying parameter blocks to optimize; " << "bailing early."; @@ -995,18 +1112,27 @@ Program* SolverImpl::CreateReducedProgram(Solver::Options* options, } if (IsSchurType(options->linear_solver_type)) { - if (!ReorderProgramForSchurTypeLinearSolver(problem_impl->parameter_map(), - linear_solver_ordering, - transformed_program.get(), - error)) { + if (!ReorderProgramForSchurTypeLinearSolver( + options->linear_solver_type, + options->sparse_linear_algebra_library_type, + problem_impl->parameter_map(), + linear_solver_ordering, + transformed_program.get(), + error)) { return NULL; } return transformed_program.release(); } - if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && - options->sparse_linear_algebra_library == SUITE_SPARSE) { - ReorderProgramForSparseNormalCholesky(transformed_program.get()); + if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + if (!ReorderProgramForSparseNormalCholesky( + options->sparse_linear_algebra_library_type, + linear_solver_ordering, + transformed_program.get(), + error)) { + return NULL; + } + return transformed_program.release(); } @@ -1030,9 +1156,32 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, } } +#ifdef CERES_NO_LAPACK + if (options->linear_solver_type == DENSE_NORMAL_CHOLESKY && + options->dense_linear_algebra_library_type == LAPACK) { + *error = "Can't use DENSE_NORMAL_CHOLESKY with LAPACK because " + "LAPACK was not enabled when Ceres was built."; + return NULL; + } + + if (options->linear_solver_type == DENSE_QR && + options->dense_linear_algebra_library_type == LAPACK) { + *error = "Can't use DENSE_QR with LAPACK because " + "LAPACK was not enabled when Ceres was built."; + return NULL; + } + + if (options->linear_solver_type == DENSE_SCHUR && + options->dense_linear_algebra_library_type == LAPACK) { + *error = "Can't use DENSE_SCHUR with LAPACK because " + "LAPACK was not enabled when Ceres was built."; + return NULL; + } +#endif + #ifdef CERES_NO_SUITESPARSE if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && - options->sparse_linear_algebra_library == SUITE_SPARSE) { + options->sparse_linear_algebra_library_type == SUITE_SPARSE) { *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITESPARSE because " "SuiteSparse was not enabled when Ceres was built."; return NULL; @@ -1053,7 +1202,7 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, #ifdef CERES_NO_CXSPARSE if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && - options->sparse_linear_algebra_library == CX_SPARSE) { + options->sparse_linear_algebra_library_type == CX_SPARSE) { *error = "Can't use SPARSE_NORMAL_CHOLESKY with CXSPARSE because " "CXSparse was not enabled when Ceres was built."; return NULL; @@ -1068,31 +1217,45 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, } #endif - if (options->linear_solver_max_num_iterations <= 0) { - *error = "Solver::Options::linear_solver_max_num_iterations is 0."; + if (options->max_linear_solver_iterations <= 0) { + *error = "Solver::Options::max_linear_solver_iterations is not positive."; return NULL; } - if (options->linear_solver_min_num_iterations <= 0) { - *error = "Solver::Options::linear_solver_min_num_iterations is 0."; + if (options->min_linear_solver_iterations <= 0) { + *error = "Solver::Options::min_linear_solver_iterations is not positive."; return NULL; } - if (options->linear_solver_min_num_iterations > - options->linear_solver_max_num_iterations) { - *error = "Solver::Options::linear_solver_min_num_iterations > " - "Solver::Options::linear_solver_max_num_iterations."; + if (options->min_linear_solver_iterations > + options->max_linear_solver_iterations) { + *error = "Solver::Options::min_linear_solver_iterations > " + "Solver::Options::max_linear_solver_iterations."; return NULL; } LinearSolver::Options linear_solver_options; linear_solver_options.min_num_iterations = - options->linear_solver_min_num_iterations; + options->min_linear_solver_iterations; linear_solver_options.max_num_iterations = - options->linear_solver_max_num_iterations; + options->max_linear_solver_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.sparse_linear_algebra_library_type = + options->sparse_linear_algebra_library_type; + linear_solver_options.dense_linear_algebra_library_type = + options->dense_linear_algebra_library_type; linear_solver_options.use_postordering = options->use_postordering; + + // Ignore user's postordering preferences and force it to be true if + // cholmod_camd is not available. This ensures that the linear + // solver does not assume that a fill-reducing pre-ordering has been + // done. +#if !defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CAMD) + if (IsSchurType(linear_solver_options.type) && + options->sparse_linear_algebra_library_type == SUITE_SPARSE) { + linear_solver_options.use_postordering = true; + } +#endif + linear_solver_options.num_threads = options->num_linear_solver_threads; options->num_linear_solver_threads = linear_solver_options.num_threads; @@ -1115,48 +1278,6 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, return LinearSolver::Create(linear_solver_options); } -bool SolverImpl::ApplyUserOrdering( - const ProblemImpl::ParameterMap& parameter_map, - const ParameterBlockOrdering* ordering, - Program* program, - string* error) { - if (ordering->NumElements() != program->NumParameterBlocks()) { - *error = StringPrintf("User specified ordering does not have the same " - "number of parameters as the problem. The problem" - "has %d blocks while the ordering has %d blocks.", - program->NumParameterBlocks(), - ordering->NumElements()); - return false; - } - - vector<ParameterBlock*>* parameter_blocks = - program->mutable_parameter_blocks(); - parameter_blocks->clear(); - - const map<int, set<double*> >& groups = - ordering->group_to_elements(); - - for (map<int, set<double*> >::const_iterator group_it = groups.begin(); - group_it != groups.end(); - ++group_it) { - const set<double*>& group = group_it->second; - for (set<double*>::const_iterator parameter_block_ptr_it = group.begin(); - parameter_block_ptr_it != group.end(); - ++parameter_block_ptr_it) { - ProblemImpl::ParameterMap::const_iterator parameter_block_it = - parameter_map.find(*parameter_block_ptr_it); - if (parameter_block_it == parameter_map.end()) { - *error = StringPrintf("User specified ordering contains a pointer " - "to a double that is not a parameter block in " - "the problem. The invalid double is in group: %d", - group_it->first); - return false; - } - parameter_blocks->push_back(parameter_block_it->second); - } - } - return true; -} // Find the minimum index of any parameter block to the given residual. // Parameter blocks that have indices greater than num_eliminate_blocks are @@ -1283,6 +1404,8 @@ CoordinateDescentMinimizer* SolverImpl::CreateInnerIterationMinimizer( const Program& program, const ProblemImpl::ParameterMap& parameter_map, Solver::Summary* summary) { + summary->inner_iterations_given = true; + scoped_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer( new CoordinateDescentMinimizer); scoped_ptr<ParameterBlockOrdering> inner_iteration_ordering; @@ -1325,9 +1448,9 @@ CoordinateDescentMinimizer* SolverImpl::CreateInnerIterationMinimizer( return NULL; } - summary->inner_iterations = true; + summary->inner_iterations_used = true; + summary->inner_iteration_time_in_seconds = 0.0; SummarizeOrdering(ordering_ptr, &(summary->inner_iteration_ordering_used)); - return inner_iteration_minimizer.release(); } @@ -1357,75 +1480,62 @@ void SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver( // CGNR currently only supports the JACOBI preconditioner. options->preconditioner_type = JACOBI; } else { - msg += StringPrintf("ITERATIVE_SCHUR with IDENTITY preconditioner " - "to CGNR with IDENTITY preconditioner."); + msg += "ITERATIVE_SCHUR with IDENTITY preconditioner" + "to CGNR with IDENTITY preconditioner."; } } LOG(WARNING) << msg; } -bool SolverImpl::ReorderProgramForSchurTypeLinearSolver( +bool SolverImpl::ApplyUserOrdering( const ProblemImpl::ParameterMap& parameter_map, - ParameterBlockOrdering* ordering, + const ParameterBlockOrdering* parameter_block_ordering, Program* program, string* error) { - // At this point one of two things is true. - // - // 1. The user did not specify an ordering - ordering has one - // group containined all the parameter blocks. - - // 2. The user specified an ordering, and the first group has - // non-zero elements. - // - // We handle these two cases in turn. - if (ordering->NumGroups() == 1) { - // If the user supplied an ordering with just one - // group, it is equivalent to the user supplying NULL as an - // ordering. Ceres is completely free to choose the parameter - // block ordering as it sees fit. For Schur type solvers, this - // means that the user wishes for Ceres to identify the e_blocks, - // which we do by computing a maximal independent set. - vector<ParameterBlock*> schur_ordering; - const int num_eliminate_blocks = ComputeSchurOrdering(*program, - &schur_ordering); + const int num_parameter_blocks = program->NumParameterBlocks(); + if (parameter_block_ordering->NumElements() != num_parameter_blocks) { + *error = StringPrintf("User specified ordering does not have the same " + "number of parameters as the problem. The problem" + "has %d blocks while the ordering has %d blocks.", + num_parameter_blocks, + parameter_block_ordering->NumElements()); + return false; + } - CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks()) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + parameter_blocks->clear(); - // Update the ordering object. - for (int i = 0; i < schur_ordering.size(); ++i) { - double* parameter_block = schur_ordering[i]->mutable_user_state(); - const int group_id = (i < num_eliminate_blocks) ? 0 : 1; - ordering->AddElementToGroup(parameter_block, group_id); - } + const map<int, set<double*> >& groups = + parameter_block_ordering->group_to_elements(); - // Apply the parameter block re-ordering. Technically we could - // call ApplyUserOrdering, but this is cheaper and simpler. - swap(*program->mutable_parameter_blocks(), schur_ordering); - } else { - // The user supplied an ordering. - if (!ApplyUserOrdering(parameter_map, ordering, program, error)) { - return false; + for (map<int, set<double*> >::const_iterator group_it = groups.begin(); + group_it != groups.end(); + ++group_it) { + const set<double*>& group = group_it->second; + for (set<double*>::const_iterator parameter_block_ptr_it = group.begin(); + parameter_block_ptr_it != group.end(); + ++parameter_block_ptr_it) { + ProblemImpl::ParameterMap::const_iterator parameter_block_it = + parameter_map.find(*parameter_block_ptr_it); + if (parameter_block_it == parameter_map.end()) { + *error = StringPrintf("User specified ordering contains a pointer " + "to a double that is not a parameter block in " + "the problem. The invalid double is in group: %d", + group_it->first); + return false; + } + parameter_blocks->push_back(parameter_block_it->second); } } - - program->SetParameterOffsetsAndIndex(); - - const int num_eliminate_blocks = - ordering->group_to_elements().begin()->second.size(); - - // Schur type solvers also require that their residual blocks be - // lexicographically ordered. - return LexicographicallyOrderResidualBlocks(num_eliminate_blocks, - program, - error); + return true; } + TripletSparseMatrix* SolverImpl::CreateJacobianBlockSparsityTranspose( const Program* program) { - // Matrix to store the block sparsity structure of + // Matrix to store the block sparsity structure of the Jacobian. TripletSparseMatrix* tsm = new TripletSparseMatrix(program->NumParameterBlocks(), program->NumResidualBlocks(), @@ -1449,6 +1559,7 @@ TripletSparseMatrix* SolverImpl::CreateJacobianBlockSparsityTranspose( // Re-size the matrix if needed. if (num_nonzeros >= tsm->max_num_nonzeros()) { + tsm->set_num_nonzeros(num_nonzeros); tsm->Reserve(2 * num_nonzeros); rows = tsm->mutable_rows(); cols = tsm->mutable_cols(); @@ -1468,34 +1579,205 @@ TripletSparseMatrix* SolverImpl::CreateJacobianBlockSparsityTranspose( return tsm; } -void SolverImpl::ReorderProgramForSparseNormalCholesky(Program* program) { -#ifndef CERES_NO_SUITESPARSE - // Set the offsets and index for CreateJacobianSparsityTranspose. +bool SolverImpl::ReorderProgramForSchurTypeLinearSolver( + const LinearSolverType linear_solver_type, + const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ProblemImpl::ParameterMap& parameter_map, + ParameterBlockOrdering* parameter_block_ordering, + Program* program, + string* error) { + if (parameter_block_ordering->NumGroups() == 1) { + // If the user supplied an parameter_block_ordering with just one + // group, it is equivalent to the user supplying NULL as an + // parameter_block_ordering. Ceres is completely free to choose the + // parameter block ordering as it sees fit. For Schur type solvers, + // this means that the user wishes for Ceres to identify the + // e_blocks, which we do by computing a maximal independent set. + vector<ParameterBlock*> schur_ordering; + const int num_eliminate_blocks = + ComputeStableSchurOrdering(*program, &schur_ordering); + + CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Update the parameter_block_ordering object. + for (int i = 0; i < schur_ordering.size(); ++i) { + double* parameter_block = schur_ordering[i]->mutable_user_state(); + const int group_id = (i < num_eliminate_blocks) ? 0 : 1; + parameter_block_ordering->AddElementToGroup(parameter_block, group_id); + } + + // We could call ApplyUserOrdering but this is cheaper and + // simpler. + swap(*program->mutable_parameter_blocks(), schur_ordering); + } else { + // The user provided an ordering with more than one elimination + // group. Trust the user and apply the ordering. + if (!ApplyUserOrdering(parameter_map, + parameter_block_ordering, + program, + error)) { + return false; + } + } + + // Pre-order the columns corresponding to the schur complement if + // possible. +#if !defined(CERES_NO_SUITESPARSE) && !defined(CERES_NO_CAMD) + if (linear_solver_type == SPARSE_SCHUR && + sparse_linear_algebra_library_type == SUITE_SPARSE) { + vector<int> constraints; + vector<ParameterBlock*>& parameter_blocks = + *(program->mutable_parameter_blocks()); + + for (int i = 0; i < parameter_blocks.size(); ++i) { + constraints.push_back( + parameter_block_ordering->GroupId( + parameter_blocks[i]->mutable_user_state())); + } + + // Renumber the entries of constraints to be contiguous integers + // as camd requires that the group ids be in the range [0, + // parameter_blocks.size() - 1]. + SolverImpl::CompactifyArray(&constraints); + + // Set the offsets and index for CreateJacobianSparsityTranspose. + program->SetParameterOffsetsAndIndex(); + // Compute a block sparse presentation of J'. + scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( + SolverImpl::CreateJacobianBlockSparsityTranspose(program)); + + SuiteSparse ss; + cholmod_sparse* block_jacobian_transpose = + ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); + + vector<int> ordering(parameter_blocks.size(), 0); + ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose, + &constraints[0], + &ordering[0]); + ss.Free(block_jacobian_transpose); + + const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); + for (int i = 0; i < program->NumParameterBlocks(); ++i) { + parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; + } + } +#endif + program->SetParameterOffsetsAndIndex(); + // Schur type solvers also require that their residual blocks be + // lexicographically ordered. + const int num_eliminate_blocks = + parameter_block_ordering->group_to_elements().begin()->second.size(); + return LexicographicallyOrderResidualBlocks(num_eliminate_blocks, + program, + error); +} +bool SolverImpl::ReorderProgramForSparseNormalCholesky( + const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ParameterBlockOrdering* parameter_block_ordering, + Program* program, + string* error) { + // Set the offsets and index for CreateJacobianSparsityTranspose. + program->SetParameterOffsetsAndIndex(); // Compute a block sparse presentation of J'. scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( SolverImpl::CreateJacobianBlockSparsityTranspose(program)); - // Order rows using AMD. - SuiteSparse ss; - cholmod_sparse* block_jacobian_transpose = - ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); + vector<int> ordering(program->NumParameterBlocks(), 0); + vector<ParameterBlock*>& parameter_blocks = + *(program->mutable_parameter_blocks()); - vector<int> ordering(program->NumParameterBlocks(), -1); - ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]); - ss.Free(block_jacobian_transpose); + if (sparse_linear_algebra_library_type == SUITE_SPARSE) { +#ifdef CERES_NO_SUITESPARSE + *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITE_SPARSE because " + "SuiteSparse was not enabled when Ceres was built."; + return false; +#else + SuiteSparse ss; + cholmod_sparse* block_jacobian_transpose = + ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); + +# ifdef CERES_NO_CAMD + // No cholmod_camd, so ignore user's parameter_block_ordering and + // use plain old AMD. + ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]); +# else + if (parameter_block_ordering->NumGroups() > 1) { + // If the user specified more than one elimination groups use them + // to constrain the ordering. + vector<int> constraints; + for (int i = 0; i < parameter_blocks.size(); ++i) { + constraints.push_back( + parameter_block_ordering->GroupId( + parameter_blocks[i]->mutable_user_state())); + } + ss.ConstrainedApproximateMinimumDegreeOrdering( + block_jacobian_transpose, + &constraints[0], + &ordering[0]); + } else { + ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, + &ordering[0]); + } +# endif // CERES_NO_CAMD + + ss.Free(block_jacobian_transpose); +#endif // CERES_NO_SUITESPARSE + + } else if (sparse_linear_algebra_library_type == CX_SPARSE) { +#ifndef CERES_NO_CXSPARSE + + // CXSparse works with J'J instead of J'. So compute the block + // sparsity for J'J and compute an approximate minimum degree + // ordering. + CXSparse cxsparse; + cs_di* block_jacobian_transpose; + block_jacobian_transpose = + cxsparse.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); + cs_di* block_jacobian = cxsparse.TransposeMatrix(block_jacobian_transpose); + cs_di* block_hessian = + cxsparse.MatrixMatrixMultiply(block_jacobian_transpose, block_jacobian); + cxsparse.Free(block_jacobian); + cxsparse.Free(block_jacobian_transpose); + + cxsparse.ApproximateMinimumDegreeOrdering(block_hessian, &ordering[0]); + cxsparse.Free(block_hessian); +#else // CERES_NO_CXSPARSE + *error = "Can't use SPARSE_NORMAL_CHOLESKY with CX_SPARSE because " + "CXSparse was not enabled when Ceres was built."; + return false; +#endif // CERES_NO_CXSPARSE + } else { + *error = "Unknown sparse linear algebra library."; + return false; + } // Apply ordering. - vector<ParameterBlock*>& parameter_blocks = - *(program->mutable_parameter_blocks()); const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); for (int i = 0; i < program->NumParameterBlocks(); ++i) { parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; } -#endif program->SetParameterOffsetsAndIndex(); + return true; +} + +void SolverImpl::CompactifyArray(vector<int>* array_ptr) { + vector<int>& array = *array_ptr; + const set<int> unique_group_ids(array.begin(), array.end()); + map<int, int> group_id_map; + for (set<int>::const_iterator it = unique_group_ids.begin(); + it != unique_group_ids.end(); + ++it) { + InsertOrDie(&group_id_map, *it, group_id_map.size()); + } + + for (int i = 0; i < array.size(); ++i) { + array[i] = group_id_map[array[i]]; + } } } // namespace internal 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 22ca6229b81..2b7ca3e3310 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h @@ -103,15 +103,6 @@ class SolverImpl { static LinearSolver* CreateLinearSolver(Solver::Options* options, string* error); - // Reorder the parameter blocks in program using the ordering. A - // return value of true indicates success and false indicates an - // error was encountered whose cause is logged to LOG(ERROR). - static bool ApplyUserOrdering(const ProblemImpl::ParameterMap& parameter_map, - const ParameterBlockOrdering* ordering, - Program* program, - string* error); - - // Reorder the residuals for program, if necessary, so that the // residuals involving e block (i.e., the first num_eliminate_block // parameter blocks) occur together. This is a necessary condition @@ -163,36 +154,67 @@ class SolverImpl { static void AlternateLinearSolverForSchurTypeLinearSolver( Solver::Options* options); + // Create a TripletSparseMatrix which contains the zero-one + // structure corresponding to the block sparsity of the transpose of + // the Jacobian matrix. + // + // Caller owns the result. + static TripletSparseMatrix* CreateJacobianBlockSparsityTranspose( + const Program* program); + + // Reorder the parameter blocks in program using the ordering + static bool ApplyUserOrdering( + const ProblemImpl::ParameterMap& parameter_map, + const ParameterBlockOrdering* parameter_block_ordering, + Program* program, + string* error); + + // Sparse cholesky factorization routines when doing the sparse + // cholesky factorization of the Jacobian matrix, reorders its + // columns to reduce the fill-in. Compute this permutation and + // re-order the parameter blocks. + // + // If the parameter_block_ordering contains more than one + // elimination group and support for constrained fill-reducing + // ordering is available in the sparse linear algebra library + // (SuiteSparse version >= 4.2.0) then the fill reducing + // ordering will take it into account, otherwise it will be ignored. + static bool ReorderProgramForSparseNormalCholesky( + const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ParameterBlockOrdering* parameter_block_ordering, + Program* program, + string* error); + // Schur type solvers require that all parameter blocks eliminated // by the Schur eliminator occur before others and the residuals be // sorted in lexicographic order of their parameter blocks. // - // If ordering has atleast two groups, then apply the ordering, - // otherwise compute a new ordering using a Maximal Independent Set - // algorithm and apply it. + // If the parameter_block_ordering only contains one elimination + // group then a maximal independent set is computed and used as the + // first elimination group, otherwise the user's ordering is used. + // + // If the linear solver type is SPARSE_SCHUR and support for + // constrained fill-reducing ordering is available in the sparse + // linear algebra library (SuiteSparse version >= 4.2.0) then + // columns of the schur complement matrix are ordered to reduce the + // fill-in the Cholesky factorization. // // Upon return, ordering contains the parameter block ordering that // was used to order the program. static bool ReorderProgramForSchurTypeLinearSolver( + const LinearSolverType linear_solver_type, + const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, const ProblemImpl::ParameterMap& parameter_map, - ParameterBlockOrdering* ordering, + ParameterBlockOrdering* parameter_block_ordering, Program* program, string* error); - // CHOLMOD when doing the sparse cholesky factorization of the - // Jacobian matrix, reorders its columns to reduce the - // fill-in. Compute this permutation and re-order the parameter - // blocks. - // - static void ReorderProgramForSparseNormalCholesky(Program* program); - - // Create a TripletSparseMatrix which contains the zero-one - // structure corresponding to the block sparsity of the transpose of - // the Jacobian matrix. - // - // Caller owns the result. - static TripletSparseMatrix* CreateJacobianBlockSparsityTranspose( - const Program* program); + // array contains a list of (possibly repeating) non-negative + // integers. Let us assume that we have constructed another array + // `p` by sorting and uniqueing the entries of array. + // CompactifyArray replaces each entry in "array" with its position + // in `p`. + static void CompactifyArray(vector<int>* array); }; } // namespace internal 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 1b19f887946..f3b96712a70 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h @@ -41,8 +41,6 @@ namespace ceres { namespace internal { -class SparseMatrixProto; - // This class defines the interface for storing and manipulating // sparse matrices. The key property that differentiates different // sparse matrices is how they are organized in memory and how the @@ -86,11 +84,6 @@ class SparseMatrix : public LinearOperator { // sparse matrix. virtual void ToDenseMatrix(Matrix* dense_matrix) const = 0; -#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 - // Write out the matrix as a sequence of (i,j,s) triplets. This // format is useful for loading the matrix into MATLAB/octave as a // sparse matrix. 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 bc1f98334ae..f1a52378e2b 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 @@ -36,11 +36,8 @@ #include <cstring> #include <ctime> -#ifndef CERES_NO_CXSPARSE -#include "cs.h" -#endif - #include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/cxsparse.h" #include "ceres/internal/eigen.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/linear_solver.h" @@ -54,14 +51,9 @@ namespace internal { SparseNormalCholeskySolver::SparseNormalCholeskySolver( const LinearSolver::Options& options) - : options_(options) { -#ifndef CERES_NO_SUITESPARSE - factor_ = NULL; -#endif - -#ifndef CERES_NO_CXSPARSE - cxsparse_factor_ = NULL; -#endif // CERES_NO_CXSPARSE + : factor_(NULL), + cxsparse_factor_(NULL), + options_(options) { } SparseNormalCholeskySolver::~SparseNormalCholeskySolver() { @@ -85,18 +77,18 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double * x) { - switch (options_.sparse_linear_algebra_library) { + switch (options_.sparse_linear_algebra_library_type) { 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; + << options_.sparse_linear_algebra_library_type; } LOG(FATAL) << "Unknown sparse linear algebra library : " - << options_.sparse_linear_algebra_library; + << options_.sparse_linear_algebra_library_type; return LinearSolver::Summary(); } @@ -133,34 +125,37 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( // 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); + cs_di* A2 = cxsparse_.TransposeMatrix(&At); + cs_di* AtA = cxsparse_.MatrixMatrixMultiply(&At, A2); cxsparse_.Free(A2); if (per_solve_options.D != NULL) { A->DeleteRows(num_cols); } - event_logger.AddEvent("Setup"); // Compute symbolic factorization if not available. if (cxsparse_factor_ == NULL) { - cxsparse_factor_ = CHECK_NOTNULL(cxsparse_.AnalyzeCholesky(AtA)); + if (options_.use_postordering) { + cxsparse_factor_ = + CHECK_NOTNULL(cxsparse_.BlockAnalyzeCholesky(AtA, + A->col_blocks(), + A->col_blocks())); + } else { + cxsparse_factor_ = + CHECK_NOTNULL(cxsparse_.AnalyzeCholeskyWithNaturalOrdering(AtA)); + } } - event_logger.AddEvent("Analysis"); - // Solve the linear system. if (cxsparse_.SolveCholesky(AtA, cxsparse_factor_, Atb.data())) { VectorRef(x, Atb.rows()) = Atb; summary.termination_type = TOLERANCE; } - event_logger.AddEvent("Solve"); cxsparse_.Free(AtA); - event_logger.AddEvent("Teardown"); return summary; } @@ -205,11 +200,13 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( if (factor_ == NULL) { if (options_.use_postordering) { - factor_ = ss_.BlockAnalyzeCholesky(&lhs, - A->col_blocks(), - A->row_blocks()); + factor_ = + CHECK_NOTNULL(ss_.BlockAnalyzeCholesky(&lhs, + A->col_blocks(), + A->row_blocks())); } else { - factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs); + factor_ = + CHECK_NOTNULL(ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs)); } } 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 ebb32e61939..61111b41b49 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 @@ -73,17 +73,13 @@ class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { 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 cs_dis* cxsparse_factor_; -#endif // CERES_NO_CXSPARSE const LinearSolver::Options options_; CERES_DISALLOW_COPY_AND_ASSIGN(SparseNormalCholeskySolver); diff --git a/extern/libmv/third_party/ceres/internal/ceres/split.h b/extern/libmv/third_party/ceres/internal/ceres/split.h index 4df48c3a7cd..2334d26037f 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/split.h +++ b/extern/libmv/third_party/ceres/internal/ceres/split.h @@ -1,4 +1,31 @@ -// Copyright 2011 Google Inc. All Rights Reserved. +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2011 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// // Author: keir@google.com (Keir Mierle) #ifndef CERES_INTERNAL_SPLIT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc index 5138b522d09..9de32fd76ad 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc @@ -33,6 +33,7 @@ #include <vector> #include "cholmod.h" +#include "ceres/compressed_col_sparse_matrix_utils.h" #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/triplet_sparse_matrix.h" @@ -172,7 +173,8 @@ cholmod_factor* SuiteSparse::AnalyzeCholeskyWithUserOrdering( return factor; } -cholmod_factor* SuiteSparse::AnalyzeCholeskyWithNaturalOrdering(cholmod_sparse* A) { +cholmod_factor* SuiteSparse::AnalyzeCholeskyWithNaturalOrdering( + cholmod_sparse* A) { cc_.nmethods = 1; cc_.method[0].ordering = CHOLMOD_NATURAL; cc_.postorder = 0; @@ -201,11 +203,12 @@ bool SuiteSparse::BlockAMDOrdering(const cholmod_sparse* A, vector<int> block_cols; vector<int> block_rows; - ScalarMatrixToBlockMatrix(A, - row_blocks, - col_blocks, - &block_rows, - &block_cols); + CompressedColumnScalarMatrixToBlockMatrix(reinterpret_cast<const int*>(A->i), + reinterpret_cast<const int*>(A->p), + row_blocks, + col_blocks, + &block_rows, + &block_cols); cholmod_sparse_struct block_matrix; block_matrix.nrow = num_row_blocks; @@ -230,88 +233,6 @@ bool SuiteSparse::BlockAMDOrdering(const cholmod_sparse* A, 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); @@ -402,6 +323,21 @@ void SuiteSparse::ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix, cholmod_amd(matrix, NULL, 0, ordering, &cc_); } +void SuiteSparse::ConstrainedApproximateMinimumDegreeOrdering( + cholmod_sparse* matrix, + int* constraints, + int* ordering) { +#ifndef CERES_NO_CAMD + cholmod_camd(matrix, NULL, 0, constraints, ordering, &cc_); +#else + LOG(FATAL) << "Congratulations you have found a bug in Ceres." + << "Ceres Solver was compiled with SuiteSparse " + << "version 4.1.0 or less. Calling this function " + << "in that case is a bug. Please contact the" + << "the Ceres Solver developers."; +#endif +} + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h index a1a4f355d76..16f298ea79c 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h @@ -33,6 +33,7 @@ #ifndef CERES_INTERNAL_SUITESPARSE_H_ #define CERES_INTERNAL_SUITESPARSE_H_ + #ifndef CERES_NO_SUITESPARSE #include <cstring> @@ -42,6 +43,29 @@ #include "ceres/internal/port.h" #include "cholmod.h" #include "glog/logging.h" +#include "SuiteSparseQR.hpp" + +// Before SuiteSparse version 4.2.0, cholmod_camd was only enabled +// if SuiteSparse was compiled with Metis support. This makes +// calling and linking into cholmod_camd problematic even though it +// has nothing to do with Metis. This has been fixed reliably in +// 4.2.0. +// +// The fix was actually committed in 4.1.0, but there is +// some confusion about a silent update to the tar ball, so we are +// being conservative and choosing the next minor version where +// things are stable. +#if (SUITESPARSE_VERSION < 4002) +#define CERES_NO_CAMD +#endif + +// UF_long is deprecated but SuiteSparse_long is only available in +// newer versions of SuiteSparse. So for older versions of +// SuiteSparse, we define SuiteSparse_long to be the same as UF_long, +// which is what recent versions of SuiteSparse do anyways. +#ifndef SuiteSparse_long +#define SuiteSparse_long UF_long +#endif namespace ceres { namespace internal { @@ -184,34 +208,43 @@ class SuiteSparse { 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); - // Find a fill reducing approximate minimum degree // ordering. ordering is expected to be large enough to hold the // ordering. void ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix, int* ordering); + + // Before SuiteSparse version 4.2.0, cholmod_camd was only enabled + // if SuiteSparse was compiled with Metis support. This makes + // calling and linking into cholmod_camd problematic even though it + // has nothing to do with Metis. This has been fixed reliably in + // 4.2.0. + // + // The fix was actually committed in 4.1.0, but there is + // some confusion about a silent update to the tar ball, so we are + // being conservative and choosing the next minor version where + // things are stable. + static bool IsConstrainedApproximateMinimumDegreeOrderingAvailable() { + return (SUITESPARSE_VERSION>4001); + } + + // Find a fill reducing approximate minimum degree + // ordering. constraints is an array which associates with each + // column of the matrix an elimination group. i.e., all columns in + // group 0 are eliminated first, all columns in group 1 are + // eliminated next etc. This function finds a fill reducing ordering + // that obeys these constraints. + // + // Calling ApproximateMinimumDegreeOrdering is equivalent to calling + // ConstrainedApproximateMinimumDegreeOrdering with a constraint + // array that puts all columns in the same elimination group. + // + // If CERES_NO_CAMD is defined then calling this function will + // result in a crash. + void ConstrainedApproximateMinimumDegreeOrdering(cholmod_sparse* matrix, + int* constraints, + int* ordering); + 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_); } @@ -237,6 +270,11 @@ class SuiteSparse { } // namespace internal } // namespace ceres +#else // CERES_NO_SUITESPARSE + +class SuiteSparse {}; +typedef void cholmod_factor; + #endif // CERES_NO_SUITESPARSE #endif // CERES_INTERNAL_SUITESPARSE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc index a09f38ee24e..824b123bc28 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 @@ -35,7 +35,6 @@ #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" @@ -83,32 +82,6 @@ TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig) CopyData(orig); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -TripletSparseMatrix::TripletSparseMatrix(const SparseMatrixProto& outer_proto) { - CHECK(outer_proto.has_triplet_matrix()); - - const TripletSparseMatrixProto& proto = outer_proto.triplet_matrix(); - CHECK(proto.has_num_rows()); - CHECK(proto.has_num_cols()); - CHECK_EQ(proto.rows_size(), proto.cols_size()); - CHECK_EQ(proto.cols_size(), proto.values_size()); - - // Initialize the matrix with the appropriate size and capacity. - max_num_nonzeros_ = 0; - set_num_nonzeros(0); - Reserve(proto.num_nonzeros()); - Resize(proto.num_rows(), proto.num_cols()); - set_num_nonzeros(proto.num_nonzeros()); - - // Copy the entries in. - for (int i = 0; i < proto.num_nonzeros(); ++i) { - rows_[i] = proto.rows(i); - cols_[i] = proto.cols(i); - values_[i] = proto.values(i); - } -} -#endif - TripletSparseMatrix& TripletSparseMatrix::operator=( const TripletSparseMatrix& rhs) { num_rows_ = rhs.num_rows_; @@ -215,22 +188,6 @@ void TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { } } -#ifndef CERES_NO_PROTOCOL_BUFFERS -void TripletSparseMatrix::ToProto(SparseMatrixProto *proto) const { - proto->Clear(); - - TripletSparseMatrixProto* tsm_proto = proto->mutable_triplet_matrix(); - tsm_proto->set_num_rows(num_rows_); - tsm_proto->set_num_cols(num_cols_); - tsm_proto->set_num_nonzeros(num_nonzeros_); - for (int i = 0; i < num_nonzeros_; ++i) { - tsm_proto->add_rows(rows_[i]); - tsm_proto->add_cols(cols_[i]); - tsm_proto->add_values(values_[i]); - } -} -#endif - void TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) { CHECK_EQ(B.num_cols(), num_cols_); Reserve(num_nonzeros_ + B.num_nonzeros_); 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 89a645bd879..4d7cde7fe9c 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 @@ -39,8 +39,6 @@ namespace ceres { namespace internal { -class SparseMatrixProto; - // An implementation of the SparseMatrix interface to store and // manipulate sparse matrices in triplet (i,j,s) form. This object is // inspired by the design of the cholmod_triplet struct used in the @@ -50,9 +48,6 @@ class TripletSparseMatrix : public SparseMatrix { TripletSparseMatrix(); TripletSparseMatrix(int num_rows, int num_cols, int max_num_nonzeros); explicit TripletSparseMatrix(const TripletSparseMatrix& orig); -#ifndef CERES_NO_PROTOCOL_BUFFERS - explicit TripletSparseMatrix(const SparseMatrixProto& proto); -#endif TripletSparseMatrix& operator=(const TripletSparseMatrix& rhs); @@ -65,9 +60,6 @@ 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_NO_PROTOCOL_BUFFERS - virtual void ToProto(SparseMatrixProto *proto) const; -#endif virtual void ToTextFile(FILE* file) const; virtual int num_rows() const { return num_rows_; } virtual int num_cols() const { return num_cols_; } 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 index 981c60a12e7..03d6c8e6b94 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.cc @@ -41,6 +41,7 @@ #include "Eigen/Core" #include "ceres/array_utils.h" #include "ceres/evaluator.h" +#include "ceres/file.h" #include "ceres/internal/eigen.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/linear_least_squares_problems.h" @@ -70,25 +71,8 @@ void TrustRegionMinimizer::EstimateScale(const SparseMatrix& jacobian, 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. - // - // Also num_eliminate_blocks is not visible to the trust region - // minimizer either. - // - // Both of these indicate that this is the wrong place for this - // code, and going forward this should needs fixing/refactoring. - return true; + sort(options_.trust_region_minimizer_iterations_to_dump.begin(), + options_.trust_region_minimizer_iterations_to_dump.end()); } void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, @@ -139,15 +123,16 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, // Do initial cost and Jacobian evaluation. double cost = 0.0; - if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), NULL, jacobian)) { + if (!evaluator->Evaluate(x.data(), + &cost, + residuals.data(), + gradient.data(), + jacobian)) { LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed."; summary->termination_type = NUMERICAL_FAILURE; return; } - summary->initial_cost = cost + summary->fixed_cost; - iteration_summary.cost = cost + summary->fixed_cost; - int num_consecutive_nonmonotonic_steps = 0; double minimum_cost = cost; double reference_cost = cost; @@ -155,17 +140,10 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, double candidate_cost = cost; double accumulated_candidate_model_cost_change = 0.0; - gradient.setZero(); - jacobian->LeftMultiply(residuals.data(), gradient.data()); + summary->initial_cost = cost + summary->fixed_cost; + iteration_summary.cost = cost + summary->fixed_cost; 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 initial_gradient_max_norm = @@ -189,8 +167,17 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, + summary->preprocessor_time_in_seconds; summary->iterations.push_back(iteration_summary); + if (options_.jacobi_scaling) { + EstimateScale(*jacobian, scale.data()); + jacobian->ScaleColumns(scale.data()); + } else { + scale.setOnes(); + } + int num_consecutive_invalid_steps = 0; + bool inner_iterations_are_enabled = options.inner_iteration_minimizer != NULL; while (true) { + bool inner_iterations_were_useful = false; if (!RunCallbacks(options.callbacks, iteration_summary, summary)) { return; } @@ -210,33 +197,38 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, 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 double strategy_start_time = WallTimeInSeconds(); TrustRegionStrategy::PerSolveOptions per_solve_options; per_solve_options.eta = options_.eta; + if (find(options_.trust_region_minimizer_iterations_to_dump.begin(), + options_.trust_region_minimizer_iterations_to_dump.end(), + iteration_summary.iteration) != + options_.trust_region_minimizer_iterations_to_dump.end()) { + per_solve_options.dump_format_type = + options_.trust_region_problem_dump_format_type; + per_solve_options.dump_filename_base = + JoinPath(options_.trust_region_problem_dump_directory, + StringPrintf("ceres_solver_iteration_%03d", + iteration_summary.iteration)); + } else { + per_solve_options.dump_format_type = TEXTFILE; + per_solve_options.dump_filename_base.clear(); + } + TrustRegionStrategy::Summary strategy_summary = strategy->ComputeStep(per_solve_options, jacobian, residuals.data(), trust_region_step.data()); + iteration_summary = IterationSummary(); + iteration_summary.iteration = summary->iterations.back().iteration + 1; iteration_summary.step_solver_time_in_seconds = WallTimeInSeconds() - 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."; - } + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; double model_cost_change = 0.0; if (strategy_summary.termination_type != FAILURE) { @@ -249,8 +241,8 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, // = -f'J * step - step' * J' * J * step / 2 model_residuals.setZero(); jacobian->RightMultiply(trust_region_step.data(), model_residuals.data()); - model_cost_change = -(residuals.dot(model_residuals) + - model_residuals.squaredNorm() / 2.0); + model_cost_change = + - model_residuals.dot(residuals + model_residuals / 2.0); if (model_cost_change < 0.0) { VLOG(1) << "Invalid step: current_cost: " << cost @@ -316,7 +308,9 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, new_cost = numeric_limits<double>::max(); } else { // Check if performing an inner iteration will make it better. - if (options.inner_iteration_minimizer != NULL) { + if (inner_iterations_are_enabled) { + ++summary->num_inner_iteration_steps; + double inner_iteration_start_time = WallTimeInSeconds(); const double x_plus_delta_cost = new_cost; Vector inner_iteration_x = x_plus_delta; Solver::Summary inner_iteration_summary; @@ -336,7 +330,23 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, VLOG(2) << "Inner iteration succeeded; current cost: " << cost << " x_plus_delta_cost: " << x_plus_delta_cost << " new_cost: " << new_cost; + const double inner_iteration_relative_progress = + 1.0 - new_cost / x_plus_delta_cost; + inner_iterations_are_enabled = + (inner_iteration_relative_progress > + options.inner_iteration_tolerance); + + inner_iterations_were_useful = new_cost < cost; + + // Disable inner iterations once the relative improvement + // drops below tolerance. + if (!inner_iterations_are_enabled) { + VLOG(2) << "Disabling inner iterations. Progress : " + << inner_iteration_relative_progress; + } } + summary->inner_iteration_time_in_seconds += + WallTimeInSeconds() - inner_iteration_start_time; } } @@ -355,7 +365,6 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, return; } - 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; @@ -392,13 +401,51 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, ? max(relative_decrease, historical_relative_decrease) : relative_decrease; + // Normally, the quality of a trust region step is measured by + // the ratio + // + // cost_change + // r = ----------------- + // model_cost_change + // + // All the change in the nonlinear objective is due to the trust + // region step so this ratio is a good measure of the quality of + // the trust region radius. However, when inner iterations are + // being used, cost_change includes the contribution of the + // inner iterations and its not fair to credit it all to the + // trust region algorithm. So we change the ratio to be + // + // cost_change + // r = ------------------------------------------------ + // (model_cost_change + inner_iteration_cost_change) + // + // In most cases this is fine, but it can be the case that the + // change in solution quality due to inner iterations is so large + // and the trust region step is so bad, that this ratio can become + // quite small. + // + // This can cause the trust region loop to reject this step. To + // get around this, we expicitly check if the inner iterations + // led to a net decrease in the objective function value. If + // they did, we accept the step even if the trust region ratio + // is small. + // + // Notice that we do not just check that cost_change is positive + // which is a weaker condition and would render the + // min_relative_decrease threshold useless. Instead, we keep + // track of inner_iterations_were_useful, which is true only + // when inner iterations lead to a net decrease in the cost. iteration_summary.step_is_successful = - iteration_summary.relative_decrease > options_.min_relative_decrease; + (inner_iterations_were_useful || + 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) { + + if (!inner_iterations_were_useful && + relative_decrease <= options_.min_relative_decrease) { iteration_summary.step_is_nonmonotonic = true; VLOG(2) << "Non-monotonic step! " << " relative_decrease: " << relative_decrease @@ -419,7 +466,7 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), - NULL, + gradient.data(), jacobian)) { summary->termination_type = NUMERICAL_FAILURE; summary->error = @@ -428,8 +475,6 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, 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) { 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 index f150594bbd2..0dcdbfef016 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.h +++ b/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.h @@ -31,6 +31,8 @@ #ifndef CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ #define CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ +#include <string> +#include "ceres/internal/port.h" #include "ceres/types.h" namespace ceres { @@ -58,8 +60,8 @@ class TrustRegionStrategy { : trust_region_strategy_type(LEVENBERG_MARQUARDT), initial_radius(1e4), max_radius(1e32), - lm_min_diagonal(1e-6), - lm_max_diagonal(1e32), + min_lm_diagonal(1e-6), + max_lm_diagonal(1e32), dogleg_type(TRADITIONAL_DOGLEG) { } @@ -73,8 +75,8 @@ class TrustRegionStrategy { // 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; + double min_lm_diagonal; + double max_lm_diagonal; // Further specify which dogleg method to use DoglegType dogleg_type; @@ -82,8 +84,22 @@ class TrustRegionStrategy { // Per solve options. struct PerSolveOptions { + PerSolveOptions() + : eta(0), + dump_filename_base(""), + dump_format_type(TEXTFILE) { + } + // Forcing sequence for inexact solves. double eta; + + // If non-empty and dump_format_type is not CONSOLE, the trust + // regions strategy will write the linear system to file(s) with + // name starting with dump_filename_base. If dump_format_type is + // CONSOLE then dump_filename_base will be ignored and the linear + // system will be written to the standard error. + string dump_filename_base; + DumpFormatType dump_format_type; }; struct Summary { diff --git a/extern/libmv/third_party/ceres/internal/ceres/types.cc b/extern/libmv/third_party/ceres/internal/ceres/types.cc index 2e19322cc76..a97f1a55e6b 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/types.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/types.cc @@ -101,7 +101,6 @@ const char* SparseLinearAlgebraLibraryTypeToString( } } - bool StringToSparseLinearAlgebraLibraryType( string value, SparseLinearAlgebraLibraryType* type) { @@ -111,6 +110,25 @@ bool StringToSparseLinearAlgebraLibraryType( return false; } +const char* DenseLinearAlgebraLibraryTypeToString( + DenseLinearAlgebraLibraryType type) { + switch (type) { + CASESTR(EIGEN); + CASESTR(LAPACK); + default: + return "UNKNOWN"; + } +} + +bool StringToDenseLinearAlgebraLibraryType( + string value, + DenseLinearAlgebraLibraryType* type) { + UpperCase(&value); + STRENUM(EIGEN); + STRENUM(LAPACK); + return false; +} + const char* TrustRegionStrategyTypeToString(TrustRegionStrategyType type) { switch (type) { CASESTR(LEVENBERG_MARQUARDT); @@ -165,6 +183,7 @@ const char* LineSearchDirectionTypeToString(LineSearchDirectionType type) { CASESTR(STEEPEST_DESCENT); CASESTR(NONLINEAR_CONJUGATE_GRADIENT); CASESTR(LBFGS); + CASESTR(BFGS); default: return "UNKNOWN"; } @@ -176,12 +195,14 @@ bool StringToLineSearchDirectionType(string value, STRENUM(STEEPEST_DESCENT); STRENUM(NONLINEAR_CONJUGATE_GRADIENT); STRENUM(LBFGS); + STRENUM(BFGS); return false; } const char* LineSearchTypeToString(LineSearchType type) { switch (type) { CASESTR(ARMIJO); + CASESTR(WOLFE); default: return "UNKNOWN"; } @@ -190,6 +211,28 @@ const char* LineSearchTypeToString(LineSearchType type) { bool StringToLineSearchType(string value, LineSearchType* type) { UpperCase(&value); STRENUM(ARMIJO); + STRENUM(WOLFE); + return false; +} + +const char* LineSearchInterpolationTypeToString( + LineSearchInterpolationType type) { + switch (type) { + CASESTR(BISECTION); + CASESTR(QUADRATIC); + CASESTR(CUBIC); + default: + return "UNKNOWN"; + } +} + +bool StringToLineSearchInterpolationType( + string value, + LineSearchInterpolationType* type) { + UpperCase(&value); + STRENUM(BISECTION); + STRENUM(QUADRATIC); + STRENUM(CUBIC); return false; } @@ -214,6 +257,27 @@ bool StringToNonlinearConjugateGradientType( return false; } +const char* CovarianceAlgorithmTypeToString( + CovarianceAlgorithmType type) { + switch (type) { + CASESTR(DENSE_SVD); + CASESTR(SPARSE_CHOLESKY); + CASESTR(SPARSE_QR); + default: + return "UNKNOWN"; + } +} + +bool StringToCovarianceAlgorithmType( + string value, + CovarianceAlgorithmType* type) { + UpperCase(&value); + STRENUM(DENSE_SVD); + STRENUM(SPARSE_CHOLESKY); + STRENUM(SPARSE_QR); + return false; +} + const char* SolverTerminationTypeToString(SolverTerminationType type) { switch (type) { CASESTR(NO_CONVERGENCE); @@ -272,4 +336,21 @@ bool IsSparseLinearAlgebraLibraryTypeAvailable( return false; } +bool IsDenseLinearAlgebraLibraryTypeAvailable( + DenseLinearAlgebraLibraryType type) { + if (type == EIGEN) { + return true; + } + if (type == LAPACK) { +#ifdef CERES_NO_LAPACK + return false; +#else + return true; +#endif + } + + LOG(WARNING) << "Unknown dense linear algebra library " << type; + return false; +} + } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility.cc b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc index fcd793c00a8..acfa45b863a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/visibility.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc @@ -153,4 +153,4 @@ Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility) { } // namespace internal } // namespace ceres -#endif +#endif // CERES_NO_SUITESPARSE 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 4b1e26af6d2..7af133905b3 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 @@ -324,8 +324,8 @@ void VisibilityBasedPreconditioner::InitEliminator( } // Update the values of the preconditioner matrix and factorize it. -bool VisibilityBasedPreconditioner::Update(const BlockSparseMatrixBase& A, - const double* D) { +bool VisibilityBasedPreconditioner::UpdateImpl(const BlockSparseMatrix& A, + const double* D) { const time_t start_time = time(NULL); const int num_rows = m_->num_rows(); CHECK_GT(num_rows, 0); 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 dae498730aa..c58b1a7a90a 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 @@ -62,7 +62,7 @@ namespace ceres { namespace internal { class BlockRandomAccessSparseMatrix; -class BlockSparseMatrixBase; +class BlockSparseMatrix; struct CompressedRowBlockStructure; class SchurEliminatorBase; @@ -123,7 +123,7 @@ class SchurEliminatorBase; // preconditioner.RightMultiply(x, y); // #ifndef CERES_NO_SUITESPARSE -class VisibilityBasedPreconditioner : public Preconditioner { +class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { public: // Initialize the symbolic structure of the preconditioner. bs is // the block structure of the linear system to be solved. It is used @@ -136,12 +136,13 @@ class VisibilityBasedPreconditioner : public Preconditioner { virtual ~VisibilityBasedPreconditioner(); // Preconditioner interface - virtual bool Update(const BlockSparseMatrixBase& A, const double* D); virtual void RightMultiply(const double* x, double* y) const; virtual int num_rows() const; friend class VisibilityBasedPreconditionerTest; + private: + virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); void ComputeClusterJacobiSparsity(const CompressedRowBlockStructure& bs); void ComputeClusterTridiagonalSparsity(const CompressedRowBlockStructure& bs); void InitStorage(const CompressedRowBlockStructure& bs); @@ -203,7 +204,7 @@ class VisibilityBasedPreconditioner : public Preconditioner { #else // SuiteSparse // If SuiteSparse is not compiled in, the preconditioner is not // available. -class VisibilityBasedPreconditioner : public Preconditioner { +class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { public: VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, const Preconditioner::Options& options) { @@ -215,7 +216,9 @@ class VisibilityBasedPreconditioner : public Preconditioner { virtual void LeftMultiply(const double* x, double* y) const {} virtual int num_rows() const { return -1; } virtual int num_cols() const { return -1; } - bool Update(const BlockSparseMatrixBase& A, const double* D) { + + private: + bool UpdateImpl(const BlockSparseMatrix& A, const double* D) { return false; } }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/wall_time.cc b/extern/libmv/third_party/ceres/internal/ceres/wall_time.cc index e63d20c0ab1..85c4417552d 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/wall_time.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/wall_time.cc @@ -86,7 +86,7 @@ void EventLogger::AddEvent(const string& event_name) { last_event_time_ = current_time; StringAppendF(&events_, - " %25s : %10.5f %10.5f\n", + " %30s : %10.5f %10.5f\n", event_name.c_str(), relative_time_delta, absolute_time_delta); diff --git a/extern/libmv/third_party/ceres/internal/ceres/wall_time.h b/extern/libmv/third_party/ceres/internal/ceres/wall_time.h index 45f65ca1aa5..37f5568a125 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/wall_time.h +++ b/extern/libmv/third_party/ceres/internal/ceres/wall_time.h @@ -32,7 +32,7 @@ #define CERES_INTERNAL_WALL_TIME_H_ #include <map> - +#include <string> #include "ceres/internal/port.h" #include "ceres/stringprintf.h" #include "glog/logging.h" |