diff options
| author | Sergey Sharybin <sergey.vfx@gmail.com> | 2016-01-04 16:11:12 +0300 |
|---|---|---|
| committer | Sergey Sharybin <sergey.vfx@gmail.com> | 2016-01-04 17:39:13 +0300 |
| commit | 6fb6a08bf84d5d16ebac35527a77bec37112494e (patch) | |
| tree | 9315b37d5022881ada9efcc8bc42a7387500ff0c /extern/ceres | |
| parent | 0b856dd97e43cf116ac136b74bc3a559c679522e (diff) | |
Move Ceres to extern/
Even tho it's currently only used by Libmv we might use it for something
else in the future. Plus, it's actually where it logically belongs to.
Diffstat (limited to 'extern/ceres')
260 files changed, 47539 insertions, 0 deletions
diff --git a/extern/ceres/CMakeLists.txt b/extern/ceres/CMakeLists.txt new file mode 100644 index 00000000000..711b2ef69f2 --- /dev/null +++ b/extern/ceres/CMakeLists.txt @@ -0,0 +1,350 @@ +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +# +# The Original Code is Copyright (C) 2012, Blender Foundation +# All rights reserved. +# +# Contributor(s): Blender Foundation, +# Sergey Sharybin +# +# ***** END GPL LICENSE BLOCK ***** + +# NOTE: This file is automatically generated by bundle.sh script +# If you're doing changes in this file, please update template +# in that script too + +set(INC + . + include + internal + config + ../gflags/src +) + +set(INC_SYS + ${EIGEN3_INCLUDE_DIRS} +) + +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_dense_matrix.cc + internal/ceres/block_random_access_diagonal_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/callbacks.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/dense_normal_cholesky_solver.cc + internal/ceres/dense_qr_solver.cc + internal/ceres/dense_sparse_matrix.cc + internal/ceres/detect_structure.cc + internal/ceres/dogleg_strategy.cc + internal/ceres/dynamic_compressed_row_jacobian_writer.cc + internal/ceres/dynamic_compressed_row_sparse_matrix.cc + internal/ceres/evaluator.cc + internal/ceres/file.cc + internal/ceres/generated/partitioned_matrix_view_d_d_d.cc + internal/ceres/generated/schur_eliminator_d_d_d.cc + internal/ceres/gradient_checking_cost_function.cc + internal/ceres/gradient_problem.cc + internal/ceres/gradient_problem_solver.cc + internal/ceres/implicit_schur_complement.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 + internal/ceres/linear_solver.cc + internal/ceres/line_search.cc + internal/ceres/line_search_direction.cc + internal/ceres/line_search_minimizer.cc + internal/ceres/line_search_preprocessor.cc + internal/ceres/local_parameterization.cc + internal/ceres/loss_function.cc + internal/ceres/low_rank_inverse_hessian.cc + internal/ceres/minimizer.cc + internal/ceres/normal_prior.cc + internal/ceres/parameter_block_ordering.cc + internal/ceres/partitioned_matrix_view.cc + internal/ceres/polynomial.cc + internal/ceres/preconditioner.cc + internal/ceres/preprocessor.cc + internal/ceres/problem.cc + internal/ceres/problem_impl.cc + internal/ceres/program.cc + internal/ceres/reorder_program.cc + internal/ceres/residual_block.cc + internal/ceres/residual_block_utils.cc + internal/ceres/schur_complement_solver.cc + internal/ceres/schur_eliminator.cc + internal/ceres/schur_jacobi_preconditioner.cc + internal/ceres/scratch_evaluate_preparer.cc + internal/ceres/solver.cc + internal/ceres/solver_utils.cc + internal/ceres/sparse_matrix.cc + internal/ceres/sparse_normal_cholesky_solver.cc + internal/ceres/split.cc + internal/ceres/stringprintf.cc + internal/ceres/triplet_sparse_matrix.cc + internal/ceres/trust_region_minimizer.cc + internal/ceres/trust_region_preprocessor.cc + internal/ceres/trust_region_strategy.cc + internal/ceres/types.cc + internal/ceres/wall_time.cc + + 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/dynamic_cost_function_to_functor.h + include/ceres/dynamic_numeric_diff_cost_function.h + include/ceres/fpclassify.h + include/ceres/gradient_checker.h + include/ceres/gradient_problem.h + include/ceres/gradient_problem_solver.h + include/ceres/internal/autodiff.h + include/ceres/internal/disable_warnings.h + include/ceres/internal/eigen.h + include/ceres/internal/fixed_array.h + include/ceres/internal/macros.h + include/ceres/internal/manual_constructor.h + include/ceres/internal/numeric_diff.h + include/ceres/internal/port.h + include/ceres/internal/reenable_warnings.h + include/ceres/internal/scoped_ptr.h + include/ceres/internal/variadic_evaluate.h + include/ceres/iteration_callback.h + include/ceres/jet.h + include/ceres/local_parameterization.h + include/ceres/loss_function.h + include/ceres/normal_prior.h + include/ceres/numeric_diff_cost_function.h + include/ceres/numeric_diff_options.h + include/ceres/ordered_groups.h + include/ceres/problem.h + include/ceres/rotation.h + include/ceres/sized_cost_function.h + include/ceres/solver.h + include/ceres/types.h + include/ceres/version.h + internal/ceres/array_utils.h + internal/ceres/blas.h + internal/ceres/block_evaluate_preparer.h + internal/ceres/block_jacobian_writer.h + internal/ceres/block_jacobi_preconditioner.h + internal/ceres/block_random_access_dense_matrix.h + internal/ceres/block_random_access_diagonal_matrix.h + internal/ceres/block_random_access_matrix.h + internal/ceres/block_random_access_sparse_matrix.h + internal/ceres/block_sparse_matrix.h + internal/ceres/block_structure.h + internal/ceres/callbacks.h + internal/ceres/casts.h + 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 + internal/ceres/dense_qr_solver.h + internal/ceres/dense_sparse_matrix.h + internal/ceres/detect_structure.h + internal/ceres/dogleg_strategy.h + internal/ceres/dynamic_compressed_row_finalizer.h + internal/ceres/dynamic_compressed_row_jacobian_writer.h + internal/ceres/dynamic_compressed_row_sparse_matrix.h + internal/ceres/evaluator.h + internal/ceres/execution_summary.h + internal/ceres/file.h + internal/ceres/gradient_checking_cost_function.h + internal/ceres/gradient_problem_evaluator.h + internal/ceres/graph_algorithms.h + internal/ceres/graph.h + internal/ceres/householder_vector.h + internal/ceres/implicit_schur_complement.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 + internal/ceres/linear_solver.h + internal/ceres/line_search_direction.h + internal/ceres/line_search.h + internal/ceres/line_search_minimizer.h + internal/ceres/line_search_preprocessor.h + internal/ceres/low_rank_inverse_hessian.h + internal/ceres/map_util.h + internal/ceres/minimizer.h + internal/ceres/mutex.h + internal/ceres/parameter_block.h + internal/ceres/parameter_block_ordering.h + internal/ceres/partitioned_matrix_view.h + internal/ceres/partitioned_matrix_view_impl.h + internal/ceres/polynomial.h + internal/ceres/preconditioner.h + internal/ceres/preprocessor.h + internal/ceres/problem_impl.h + internal/ceres/program_evaluator.h + internal/ceres/program.h + internal/ceres/random.h + internal/ceres/reorder_program.h + internal/ceres/residual_block.h + internal/ceres/residual_block_utils.h + internal/ceres/schur_complement_solver.h + internal/ceres/schur_eliminator.h + 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_utils.h + internal/ceres/sparse_matrix.h + internal/ceres/sparse_normal_cholesky_solver.h + internal/ceres/split.h + internal/ceres/stl_util.h + internal/ceres/stringprintf.h + internal/ceres/suitesparse.h + internal/ceres/triplet_sparse_matrix.h + internal/ceres/trust_region_minimizer.h + internal/ceres/trust_region_preprocessor.h + internal/ceres/trust_region_strategy.h + internal/ceres/visibility_based_preconditioner.h + internal/ceres/wall_time.h +) + +if(WITH_LIBMV_SCHUR_SPECIALIZATIONS) + list(APPEND SRC + internal/ceres/generated/partitioned_matrix_view_2_2_2.cc + internal/ceres/generated/partitioned_matrix_view_2_2_3.cc + internal/ceres/generated/partitioned_matrix_view_2_2_4.cc + internal/ceres/generated/partitioned_matrix_view_2_2_d.cc + internal/ceres/generated/partitioned_matrix_view_2_3_3.cc + internal/ceres/generated/partitioned_matrix_view_2_3_4.cc + internal/ceres/generated/partitioned_matrix_view_2_3_6.cc + internal/ceres/generated/partitioned_matrix_view_2_3_9.cc + internal/ceres/generated/partitioned_matrix_view_2_3_d.cc + internal/ceres/generated/partitioned_matrix_view_2_4_3.cc + internal/ceres/generated/partitioned_matrix_view_2_4_4.cc + internal/ceres/generated/partitioned_matrix_view_2_4_8.cc + internal/ceres/generated/partitioned_matrix_view_2_4_9.cc + internal/ceres/generated/partitioned_matrix_view_2_4_d.cc + internal/ceres/generated/partitioned_matrix_view_2_d_d.cc + internal/ceres/generated/partitioned_matrix_view_4_4_2.cc + internal/ceres/generated/partitioned_matrix_view_4_4_3.cc + internal/ceres/generated/partitioned_matrix_view_4_4_4.cc + internal/ceres/generated/partitioned_matrix_view_4_4_d.cc + internal/ceres/generated/schur_eliminator_2_2_2.cc + internal/ceres/generated/schur_eliminator_2_2_3.cc + internal/ceres/generated/schur_eliminator_2_2_4.cc + internal/ceres/generated/schur_eliminator_2_2_d.cc + internal/ceres/generated/schur_eliminator_2_3_3.cc + internal/ceres/generated/schur_eliminator_2_3_4.cc + internal/ceres/generated/schur_eliminator_2_3_6.cc + internal/ceres/generated/schur_eliminator_2_3_9.cc + internal/ceres/generated/schur_eliminator_2_3_d.cc + internal/ceres/generated/schur_eliminator_2_4_3.cc + internal/ceres/generated/schur_eliminator_2_4_4.cc + internal/ceres/generated/schur_eliminator_2_4_8.cc + internal/ceres/generated/schur_eliminator_2_4_9.cc + internal/ceres/generated/schur_eliminator_2_4_d.cc + internal/ceres/generated/schur_eliminator_2_d_d.cc + internal/ceres/generated/schur_eliminator_4_4_2.cc + internal/ceres/generated/schur_eliminator_4_4_3.cc + internal/ceres/generated/schur_eliminator_4_4_4.cc + internal/ceres/generated/schur_eliminator_4_4_d.cc + ) +else() + add_definitions(-DCERES_RESTRICT_SCHUR_SPECIALIZATION) +endif() + +if(WIN32) + list(APPEND INC + ../glog/src/windows + ) +else() + list(APPEND INC + ../glog/src + ) +endif() + +add_definitions(${GFLAGS_DEFINES}) +add_definitions(${GLOG_DEFINES}) +add_definitions(${CERES_DEFINES}) + +add_definitions( + -DCERES_HAVE_PTHREAD + -DCERES_NO_SUITESPARSE + -DCERES_NO_CXSPARSE + -DCERES_NO_LAPACK + -DCERES_HAVE_RWLOCK +) + +if(WITH_OPENMP) + add_definitions( + -DCERES_USE_OPENMP + ) +endif() + +TEST_UNORDERED_MAP_SUPPORT() +if(HAVE_STD_UNORDERED_MAP_HEADER) + if(HAVE_UNORDERED_MAP_IN_STD_NAMESPACE) + add_definitions(-DCERES_STD_UNORDERED_MAP) + else() + if(HAVE_UNORDERED_MAP_IN_TR1_NAMESPACE) + add_definitions(-DCERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE) + else() + add_definitions(-DCERES_NO_UNORDERED_MAP) + message(STATUS "Replacing unordered_map/set with map/set (warning: slower!)") + endif() + endif() +else() + if(HAVE_UNORDERED_MAP_IN_TR1_NAMESPACE) + add_definitions(-DCERES_TR1_UNORDERED_MAP) + else() + add_definitions(-DCERES_NO_UNORDERED_MAP) + message(STATUS "Replacing unordered_map/set with map/set (warning: slower!)") + endif() +endif() + +blender_add_lib(extern_ceres "${SRC}" "${INC}" "${INC_SYS}") diff --git a/extern/ceres/ChangeLog b/extern/ceres/ChangeLog new file mode 100644 index 00000000000..0e6c195174c --- /dev/null +++ b/extern/ceres/ChangeLog @@ -0,0 +1,659 @@ +commit aef9c9563b08d5f39eee1576af133a84749d1b48 +Author: Alessandro Gentilini <agentilini@gmail.com> +Date: Tue Oct 6 20:43:45 2015 +0200 + + Add test for Bessel functions. + + Change-Id: Ief5881e8027643d7ef627e60a88fdbad17f3d884 + +commit 49c86018e00f196c4aa9bd25daccb9919917efee +Author: Alessandro Gentilini <agentilini@gmail.com> +Date: Wed Sep 23 21:59:44 2015 +0200 + + Add Bessel functions in order to use them in residual code. + + See "How can I use the Bessel function in the residual function?" at + https://groups.google.com/d/msg/ceres-solver/Vh1gpqac8v0/NIK1EiWJCAAJ + + Change-Id: I3e80d9f9d1cadaf7177076e493ff46ace5233b76 + +commit dfb201220c034fde00a242d0533bef3f73b2907d +Author: Simon Rutishauser <simon.rutishauser@pix4d.com> +Date: Tue Oct 13 07:33:58 2015 +0200 + + Make miniglog threadsafe on non-windows system by using + localtime_r() instead of localtime() for time formatting + + Change-Id: Ib8006c685cd8ed4f374893bef56c4061ca2c9747 + +commit 41455566ac633e55f222bce7c4d2cb4cc33d5c72 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Mon Sep 28 22:43:42 2015 +0100 + + Remove link-time optimisation (LTO). + + - On GCC 4.9+ although GCC supports LTO, it requires use of the + non-default gcc-ar & gcc-ranlib. Whilst we can ensure Ceres is + compiled with these, doing so with GCC 4.9 causes multiple definition + linker errors of static ints inside Eigen when compiling the tests + and examples when they are not also built with LTO. + - On OS X (Xcode 6 & 7) after the latest update to gtest, if LTO + is used when compiling the tests (& examples), two tests fail + due to typeinfo::operator== (things are fine if only Ceres itself is + compiled with LTO). + - This patch disables LTO for all compilers. It should be revisited when + the performance is more stable across our supported compilers. + + Change-Id: I17b52957faefbdeff0aa40846dc9b342db1b02e3 + +commit 89c40005bfceadb4163bd16b7464b3c2ce740daf +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sun Sep 27 13:37:26 2015 +0100 + + Only use LTO when compiling Ceres itself, not tests or examples. + + - If Ceres is built as a shared library, and LTO is enabled for Ceres + and the tests, then type_info::operator==() incorrectly returns false + in gtests' CheckedDowncastToActualType() in the following tests: + -- levenberg_marquardt_strategy_test. + -- gradient_checking_cost_function_test. + on at least Xcode 6 & 7 as reported here: + https://github.com/google/googletest/issues/595. + - This does not appear to be a gtest issue, but is perhaps an LLVM bug + or an RTTI shared library issue. Either way, disabling the use of + LTO when compiling the test application resolves the issue. + - Allow LTO to be enabled for GCC, if it is supported. + - Add CMake function to allow easy appending to target properties s/t + Ceres library-specific compile flags can be iteratively constructed. + + Change-Id: I923e6aae4f7cefa098cf32b2f8fc19389e7918c9 + +commit 0794f41cca440f7f65d9a44e671f66f6e498ef7c +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sat Sep 26 14:10:15 2015 -0700 + + Documentation updates. + + 1. Fix a typo in the Trust Region algorithm. + 2. Add ARL in the list of users. + 3. Update the version history. + + Change-Id: Ic286e8ef1a71af07f3890b7592dd3aed9c5f87ce + +commit 90e32a8dc437dfb0e6747ce15a1f3193c13b7d5b +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Mon Sep 21 21:08:25 2015 +0100 + + Use old minimum iOS version flags on Xcode < 7.0. + + - The newer style, which are more specific and match the SDK names + are not available on Xcode < 7.0. + + Change-Id: I2f07a0365183d2781157cdb05fd49b30ae001ac5 + +commit 26cd5326a1fb99ae02c667eab9942e1308046984 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Mon Sep 21 10:16:01 2015 +0100 + + Add gtest-specific flags when building/using as a shared library. + + - Currently these flags are only used to define the relevant DLL export + prefix for Windows. + + Change-Id: I0c05207b512cb4a985390aefc779b91febdabb38 + +commit c4c79472112a49bc1340da0074af2d15b1c89749 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sun Sep 20 18:26:59 2015 +0100 + + Clean up iOS.cmake to use xcrun/xcodebuild & libtool. + + - Substantial cleanup of iOS.cmake to use xcrun & xcodebuild to + determine the SDK & tool paths. + - Use libtool -static to link libraries instead of ar + ranlib, which + is not compatible with Xcode 7+, this change should be backwards + compatible to at least Xcode 6. + - Force locations of unordered_map & shared_ptr on iOS to work around + check_cxx_source_compiles() running in a forked CMake instance without + access to the variables (IOS_PLATFORM) defined by the user. + - Minor CMake style updates. + + Change-Id: I5f83a60607db34d461ebe85f9dce861f53d98277 + +commit 155765bbb358f1d19f072a4b54825faf1c059910 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed Sep 16 06:56:08 2015 -0700 + + Import the latest version of gtest and gmock. + + Change-Id: I4b686c44bba823cab1dae40efa99e31340d2b52a + +commit 0c4647b8f1496c97c6b9376d9c49ddc204aa08dd +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Wed Sep 16 20:01:11 2015 +0100 + + Remove FAQ about increasing inlining threshold for Clang. + + - Changing the inlining threshold for Clang as described has a minimal + effect on user performance. + - The problem that originally prompted the belief that it did was + due to an erroneous CXX flag configuration (in user code). + + Change-Id: I03017241c0f87b8dcefb8c984ec3b192afd97fc2 + +commit f4b768b69afcf282568f9ab3a3f0eb8078607468 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Mon Sep 14 13:53:24 2015 -0700 + + Lint changes from William Rucklidge + + Change-Id: I0dac2549a8fa2bfd12f745a8d8a0db623b7ec1ac + +commit 5f2f05c726443e35767d677daba6d25dbc2d7ff8 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Fri Sep 11 22:19:38 2015 -0700 + + Refactor system_test + + 1. Move common test infrastructure into test_util. + 2. system_test now only contains powells function. + 3. Add bundle_adjustment_test. + + Instead of a single function which computes everything, + there is now a test for each solver configuration which + uses the reference solution computed by the fixture. + + Change-Id: I16a9a9a83a845a7aaf28762bcecf1a8ff5aee805 + +commit 1936d47e213142b8bf29d3f548905116092b093d +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Tue Sep 8 23:27:42 2015 +0100 + + Revert increased inline threshold (iff Clang) to exported Ceres target. + + - Increasing the inline threshold results in very variable performance + improvements, and could potentially confuse users if they are trying + to set the inline threshold themselves. + - As such, we no longer export our inline threshold configuration for + Clang, but instead document how to change it in the FAQs. + + Change-Id: I88e2e0001e4586ba2718535845ed1e4b1a5b72bc + +commit a66d89dcda47cefda83758bfb9e7374bec4ce866 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sat Sep 5 16:50:20 2015 -0700 + + Get ready for 1.11.0RC1 + + Update version numbers. + Drop CERES_VERSION_ABI macro. + + Change-Id: Ib3eadabb318afe206bb196a5221b195d26cbeaa0 + +commit 1ac3dd223c179fbadaed568ac532af4139c75d84 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sat Sep 5 15:30:01 2015 -0700 + + Fix a bug in CompressedRowSparseMatrix::AppendRows + + The test for CompressedRowSparseMatrix::AppendRows tries to add + a matrix of size zero, which results in an invalid pointer deferencing + even though that pointer is never written to. + + Change-Id: I97dba37082bd5dad242ae1af0447a9178cd92027 + +commit 67622b080c8d37b5e932120a53d4ce76b80543e5 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sat Sep 5 13:18:38 2015 -0700 + + Fix a pointer access bug in Ridders' algorithm. + + A pointer to an Eigen matrix was being used as an array. + + Change-Id: Ifaea14fa3416eda5953de49afb78dc5a6ea816eb + +commit 5742b7d0f14d2d170054623ccfee09ea214b8ed9 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed Aug 26 09:24:33 2015 -0700 + + Improve performance of SPARSE_NORMAL_CHOLESKY + dynamic_sparsity + + The outer product computation logic in SparseNormalCholeskySolver + does not work well with dynamic sparsity. The overhead of computing + the sparsity pattern of the normal equations is only amortized if + the sparsity is constant. If the sparsity can change from call to call + SparseNormalCholeskySolver will actually be more expensive. + + For Eigen and for CXSparse we now explicitly compute the normal + equations using their respective matrix-matrix product routines and solve. + Change-Id: Ifbd8ed78987cdf71640e66ed69500442526a23d4 + +commit d0b6cf657d6ef0dd739e958af9a5768f2eecfd35 +Author: Keir Mierle <mierle@gmail.com> +Date: Fri Sep 4 18:43:41 2015 -0700 + + Fix incorrect detect structure test + + Change-Id: I7062f3639147c40b57947790d3b18331a39a366b + +commit 0e8264cc47661651a11e2dd8570c210082963545 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sat Aug 22 16:23:05 2015 +0100 + + Add increased inline threshold (iff Clang) to exported Ceres target. + + - When compiled with Clang, Ceres and all of the examples are compiled + with an increased inlining-threshold, as the default value can result + in poor Eigen performance. + - Previously, client code using Ceres would typically not use an + increased inlining-threshold (unless the user has specifically added + it themselves). However, increasing the inlining threshold can result + in significant performance improvements in auto-diffed CostFunctions. + - This patch adds the inlining-threshold flags to the interface flags + for the Ceres CMake target s/t any client code using Ceres (via + CMake), and compiled with Clang, will now be compiled with the same + increased inlining threshold as used by Ceres itself. + + Change-Id: I31e8f1abfda140d22e85bb48aa57f028a68a415e + +commit a1b3fce9e0a4141b973f6b4dd9b08c4c13052d52 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Mon Aug 31 14:14:56 2015 +0100 + + Add optional export of Ceres build directory to new features list. + + Change-Id: I6f1e42b41957ae9cc98fd9dcd1969ef64c4cd96f + +commit e46777d8df068866ef80902401a03e29348d11ae +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Mon Aug 31 12:41:54 2015 +0100 + + Credit reporters of buildsystem bugs in version history. + + Change-Id: I16fe7973534cd556d97215e84268ae0b8ec4e11a + +commit 01548282cb620e5e3ac79a63a391cd0afd5433e4 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun Aug 30 22:29:27 2015 -0700 + + Update the version history. + + Change-Id: I29873bed31675e0108f1a44f53f7bc68976b7f98 + +commit 2701429f770fce69ed0c77523fa43d7bc20ac6dc +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun Aug 30 21:33:57 2015 -0700 + + Use Eigen::Dynamic instead of ceres::DYNAMIC in numeric_diff.h + + Change-Id: Iccb0284a8fb4c2160748dfae24bcd595f1d4cb5c + +commit 4f049db7c2a3ee8cf9910c6eac96be6a28a5999c +Author: Tal Ben-Nun <tbennun@gmail.com> +Date: Wed May 13 15:43:51 2015 +0300 + + Adaptive numeric differentiation using Ridders' method. + + This method numerically computes function derivatives in different + scales, extrapolating between intermediate results to conserve function + evaluations. Adaptive differentiation is essential to produce accurate + results for functions with noisy derivatives. + + Full changelist: + -Created a new type of NumericDiffMethod (RIDDERS). + -Implemented EvaluateRiddersJacobianColumn in NumericDiff. + -Created unit tests with f(x) = x^2 + [random noise] and + f(x) = exp(x). + + Change-Id: I2d6e924d7ff686650272f29a8c981351e6f72091 + +commit 070bba4b43b4b7449628bf456a10452fd2b34d28 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue Aug 25 13:37:33 2015 -0700 + + Lint fixes from William Rucklidge + + Change-Id: I719e8852859c970091df842e59c44e02e2c65827 + +commit 887a20ca7f02a1504e35f7cabbdfb2e0842a0b0b +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Wed Aug 12 21:41:43 2015 +0100 + + Build position independent code when compiling Ceres statically. + + - Previously, when Ceres was built as a static library we did not + compile position independent code. This means that the resulting + static library could not be linked against shared libraries, but + could be used by executables. + - To enable the use of a static Ceres library by other shared libraries + as reported in [1], the static library must be generated from + position independent code (except on Windows, where PIC does not + apply). + + [1] https://github.com/Itseez/opencv_contrib/pull/290#issuecomment-130389471 + + Change-Id: I99388f1784ece688f91b162d009578c5c97ddaf6 + +commit 860bba588b981a5718f6b73e7e840e5b8757fe65 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue Aug 25 09:43:21 2015 -0700 + + Fix a bug in DetectStructure + + The logic for determing static/dynamic f-block size in + DetectStructure was broken in a corner case, where the very first + row block which was used to initialize the f_block_size contained + more than one f blocks of varying sizes. The way the if block + was structured, no iteration was performed on the remaining + f-blocks and the loop failed to detect that the f-block size + was actually changing. + + If in the remaining row blocks, there were no row blocks + with varying f-block sizes, the function will erroneously + return a static f-block size. + + Thanks to Johannes Schonberger for providing a reproduction for this + rather tricky corner case. + + Change-Id: Ib442a041d8b7efd29f9653be6a11a69d0eccd1ec + +commit b0cbc0f0b0a22f01724b7b647a4a94db959cc4e4 +Author: Johannes Schönberger <hannesschoenberger@gmail.com> +Date: Thu Aug 20 14:21:30 2015 -0400 + + Reduce memory footprint of SubsetParameterization + + Change-Id: If113cb4696d5aef3e50eed01fba7a3d4143b7ec8 + +commit ad2a99777786101411a971e59576ca533a297013 +Author: Sergey Sharybin <sergey.vfx@gmail.com> +Date: Sat Aug 22 11:18:45 2015 +0200 + + Fix for reoder program unit test when built without suitesparse + + This commit fixes failure of reorder_program_test when Ceres is built without + any suitesparse. + + Change-Id: Ia23ae8dfd20c482cb9cd1301f17edf9a34df3235 + +commit 4bf3868beca9c17615f72ec03730cddb3676acaa +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun Aug 9 15:24:45 2015 -0700 + + Fix a bug in the Schur eliminator + + The schur eliminator treats rows with e blocks and row with + no e blocks separately. The template specialization logic only + applies to the rows with e blocks. + + So, in cases where the rows with e-blocks have a fixed size f-block + but the rows without e-blocks have f-blocks of varying sizes, + DetectStructure will return a static f-block size, but we need to be + careful that we do not blindly use that static f-block size everywhere. + + This patch fixes a bug where such care was not being taken, where + it was assumed that the static f-block size could be assumed for all + f-block sizes. + + A new test is added, which triggers an exception in debug mode. In + release mode this error does not present itself, due to a peculiarity + of the way Eigen works. + + Thanks to Werner Trobin for reporting this bug. + + Change-Id: I8ae7aabf8eed8c3f9cf74b6c74d632ba44f82581 + +commit 1635ce726078f00264b89d7fb6e76fd1c2796e59 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed Aug 19 00:26:02 2015 -0700 + + Fix a bug in the reordering code. + + When the user provides an ordering which starts at a non-zero group id, + or has gaps in the groups, then CAMD, the algorithm used to reorder + the program can crash or return garbage results. + + The solution is to map the ordering into grouping constraints, and then + to re-number the groups to be contiguous using a call to + MapValuesToContiguousRange. This was already done for CAMD based + ordering for Schur type solvers, but was not done for SPARSE_NORMAL_CHOLESKY. + + Thanks to Bernhard Zeisl for not only reporting the bug but also + providing a reproduction. + + Change-Id: I5cfae222d701dfdb8e1bda7f0b4670a30417aa89 + +commit 4c3f8987e7f0c51fd367cf6d43d7eb879e79589f +Author: Simon Rutishauser <simon.rutishauser@pix4d.com> +Date: Thu Aug 13 11:10:44 2015 +0200 + + Add missing CERES_EXPORT to ComposedLoss + + Change-Id: Id7db388d41bf53e6e5704039040c9d2c6bf4c29c + +commit 1a740cc787b85b883a0703403a99fe49662acb79 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue Aug 11 18:08:05 2015 -0700 + + Add the option to use numeric differentiation to nist and more_garbow_hillstrom + + Change-Id: If0a5caef90b524dcf5e2567c5b681987f5459401 + +commit ea667ede5c038d6bf3d1c9ec3dbdc5072d1beec6 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sun Aug 9 16:56:13 2015 +0100 + + Fix EIGENSPARSE option help s/t it displays in CMake ncurses GUI. + + - Shorten description for EIGENSPARSE to a single line, as otherwise + it is not correctly displayed in the ncurses CMake GUI. + - Made explicit in description that this results in an LGPL licensed + version of Ceres (this is also made clear in the CMake log output if + EIGENSPARSE is enabled). + + Change-Id: I11678a9cbc7a817133c22128da01055a3cb8a26d + +commit a14ec27fb28ab2e8d7f1c9d88e41101dc6c0aab5 +Author: Richard Stebbing <richie.stebbing@gmail.com> +Date: Fri Aug 7 08:42:03 2015 -0700 + + Fix SparseNormalCholeskySolver with dynamic sparsity. + + The previous implementation incorrectly cached the outer product matrix + pattern even when `dynamic_sparsity = true`. + + Change-Id: I1e58315a9b44f2f457d07c56b203ab2668bfb8a2 + +commit 3dd7fced44ff00197fa9fcb1f2081d12be728062 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sun Aug 9 16:38:50 2015 +0100 + + Remove legacy dependency detection macros. + + - Before the new CMake buildsystem in 1.8, Ceres used non-standard + HINTS variables for dependencies. For backwards compatibility CMake + macros were added to translate these legacy variables into the new + (standard) variables. + - As it has now been multiple releases since the legacy variables + were used and they no longer appear in any of the documentation + support for them has now expired. + + Change-Id: I2cc72927ed711142ba7943df334ee008181f86a2 + +commit 8b32e258ccce1eed2a50bb002add16cad13aff1e +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sun Aug 9 15:42:39 2015 +0100 + + Fix failed if() condition expansion if gflags is not found. + + - If a CMake-ified version of gflags is not detected, then + gflags_LIBRARIES is not set and the TARGET condition within a + multiconditional if() statement prevents configuration. + + Change-Id: Ia92e97523d7a1478ab36539726b9540d7cfee5d0 + +commit cc8d47aabb9d63ba4588ba7295058a6191c2df83 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sun Aug 9 15:18:42 2015 +0100 + + Update all CMake to lowercase function name style. + + - Updated to new CMake style where function names are all lowercase, + this will be backwards compatible as CMake function names are + case insensitive. + - Updated using Emacs' M-x unscreamify-cmake-buffer. + + Change-Id: If7219816f560270e59212813aeb021353a64a0e2 + +commit 1f106904c1f47460c35ac03258d6506bb2d60838 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sun Aug 9 14:55:02 2015 +0100 + + Update minimum iOS version to 7.0 for shared_ptr/unordered_map. + + - In order to correctly detect shared_ptr (& unordered_map) + the iOS version must be >= 7.0 (Xcode 5.0+). This only affects the + SIMULATOR(64) platform builds, as the OS (device) build uses the + latest SDK which is now likely 8.0+. + + Change-Id: Iefec8f03408b8cdc7a495f442ebba081f800adb0 + +commit 16ecd40523a408e7705c9fdb0e159cef2007b8ab +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sat Aug 8 17:32:31 2015 +0100 + + Fix bug in gflags' <= 2.1.2 exported CMake configuration. + + - gflags <= 2.1.2 has a bug in its exported gflags-config.cmake: + https://github.com/gflags/gflags/issues/110 whereby it sets + gflags_LIBRARIES to a non-existent 'gflags' target. + - This causes linker errors if gflags is installed in a non-standard + location (as otherwise CMake resolves gflags to -lgflags which + links if gflags is installed somewhere on the current path). + - We now check for this case, and search for the correct gflags imported + target and update gflags_LIBRARIES to reference it if found, otherwise + proceed on to the original manual search to try to find gflags. + + Change-Id: Iceccc3ee53c7c2010e41cc45255f966e7b13d526 + +commit 56be8de007dfd65ed5a31c795eb4a08ad765f411 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Thu Jun 25 21:31:00 2015 +0100 + + Add docs for new CXX11 option & mask option for Windows. + + - The CXX11 option has no effect on Windows, as there, any new C++11 + features are enabled by default, as such to avoid confusion we only + present the option for non-Windows. + + Change-Id: I38925ae3bb8c16682d404468ba95c611a519b9b9 + +commit cf863b6415ac4dbf3626e70adeac1ac0f3d87ee5 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu Aug 6 14:52:18 2015 -0700 + + Remove the spec file needed for generating RPMs. + + Now that ceres is part of RawHide, there is no need to carry + this spec file with the ceres distribution. + + Change-Id: Icc400b9874ba05ba05b353e2658f1de94c72299e + +commit 560940fa277a469c1ab34f1aa303ff1af9c3cacf +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sat Jul 11 22:21:31 2015 -0700 + + A refactor of the cubic interpolation code + + 1. Push the boundary handling logic into the underlying array + object. This has two very significant impacts: + + a. The interpolation code becomes extremely simple to write + and to test. + + b. The user has more flexibility in implementing how out of bounds + values are handled. We provide one default implementation. + + Change-Id: Ic2f6cf9257ce7110c62e492688e5a6c8be1e7df2 + +commit dfdf19e111c2b0e6daeb6007728ec2f784106d49 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed Aug 5 15:20:57 2015 -0700 + + Lint cleanup from Jim Roseborough + + Change-Id: Id6845c85644d40e635ed196ca74fc51a387aade4 + +commit 7444f23ae245476a7ac8421cc2f88d6947fd3e5f +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Mon Aug 3 12:22:44 2015 -0700 + + Fix a typo in small_blas.h + + The reason this rather serious looking typo has not + caused any problems uptil now is because NUM_ROW_B is + computed but never actually used. + + Thanks to Werner Trobin for pointing this out. + + Change-Id: Id2b4d9326ec21baec8a85423e3270aefbafb611e + +commit 5a48b92123b30a437f031eb24b0deaadc8f60d26 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sat Jul 4 17:59:52 2015 +0100 + + Export Ceres build directory into local CMake package registry. + + - Optionally use CMake's export() functionality to export the Ceres + build directory as a package into the local CMake package registry. + - This enables the detection & use of Ceres from CMake *without* + requiring that Ceres be installed. + + Change-Id: Ib5a7588446f490e1b405878475b6b1dd13accd1f + +commit d9790e77894ea99d38137d359d6118315b2d1601 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun Jul 12 19:39:47 2015 -0700 + + Add ProductParameterization + + Often a parameter block is the Cartesian product of a number of + manifolds. For example, a rigid transformation SE(3) = SO(3) x R^3 + In such cases, where you have the local parameterization + of the individual manifolds available, + ProductParameterization can be used to construct a local + parameterization of the cartesian product. + + Change-Id: I4b5bcbd2407a38739c7725b129789db5c3d65a20 + +commit 7b4fb69dad49eaefb5d2d47ef0d76f48ad7fef73 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sun Jun 28 21:43:46 2015 +0100 + + Cleanup FindGflags & use installed gflags CMake config if present. + + - Split out gflags namespace detection methods: + check_cxx_source_compiles() & regex, into separate functions. + - Use installed/exported gflags CMake configuration (present for + versions >= 2.1) if available, unless user expresses a preference not + to, or specifies search directories, in which case fall back to manual + search for components. + -- Prefer installed gflags CMake configurations over exported gflags + build directories on all OSs. + - Remove custom version of check_cxx_source_compiles() that attempted + to force the build type of the test project. This only worked for + NMake on Windows, not MSVC as msbuild ignored our attempts to force + the build type. Now we always use the regex method on Windows if + we cannot find an installed gflags CMake configuration which works + even on MSVC by bypassing msbuild. + - Add default search paths for gflags on Windows. + + Change-Id: I083b267d97a7a5838a1314f3d41a61ae48d5a2d7 + +commit b3063c047906d4a44503dc0187fdcbbfcdda5f38 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Wed Jul 15 20:56:56 2015 +0100 + + Add default glog install location on Windows to search paths. + + Change-Id: I083d368be48986e6780c11460f5a07b2f3b6c900 diff --git a/extern/ceres/LICENSE b/extern/ceres/LICENSE new file mode 100644 index 00000000000..2e3ead5ed45 --- /dev/null +++ b/extern/ceres/LICENSE @@ -0,0 +1,27 @@ +Ceres Solver - A fast non-linear least squares minimizer +Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +http://code.google.com/p/ceres-solver/ + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +* Neither the name of Google Inc. nor the names of its contributors may be + used to endorse or promote products derived from this software without + specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/extern/ceres/README b/extern/ceres/README new file mode 100644 index 00000000000..8dd8ccf91a1 --- /dev/null +++ b/extern/ceres/README @@ -0,0 +1,3 @@ +Ceres Solver - A non-linear least squares minimizer +================================================== +Please see ceres.pdf in docs/ for a tutorial and reference. diff --git a/extern/ceres/bundle.sh b/extern/ceres/bundle.sh new file mode 100755 index 00000000000..0eaf00f3989 --- /dev/null +++ b/extern/ceres/bundle.sh @@ -0,0 +1,198 @@ +#!/bin/sh + +if [ "x$1" = "x--i-really-know-what-im-doing" ] ; then + echo Proceeding as requested by command line ... +else + echo "*** Please run again with --i-really-know-what-im-doing ..." + exit 1 +fi + +repo="https://ceres-solver.googlesource.com/ceres-solver" +branch="master" +#tag="1.4.0" +tag="" +tmp=`mktemp -d` +checkout="$tmp/ceres" + +GIT="git --git-dir $tmp/ceres/.git --work-tree $checkout" + +git clone $repo $checkout + +if [ $branch != "master" ]; then + $GIT checkout -t remotes/origin/$branch +else + if [ "x$tag" != "x" ]; then + $GIT checkout $tag + fi +fi + +$GIT log -n 50 > ChangeLog + +for p in `cat ./patches/series`; do + echo "Applying patch $p..." + cat ./patches/$p | patch -d $tmp/ceres -p1 +done + +find include -type f -not -iwholename '*.svn*' -exec rm -rf {} \; +find internal -type f -not -iwholename '*.svn*' -exec rm -rf {} \; + +cat "files.txt" | while read f; do + mkdir -p `dirname $f` + cp $tmp/ceres/$f $f +done + +rm -rf $tmp + +sources=`find ./include ./internal -type f -iname '*.cc' -or -iname '*.cpp' -or -iname '*.c' | sed -r 's/^\.\//\t/' | \ + grep -v -E 'schur_eliminator_[0-9]_[0-9d]_[0-9d].cc' | \ + grep -v -E 'partitioned_matrix_view_[0-9]_[0-9d]_[0-9d].cc' | sort -d` +generated_sources=`find ./include ./internal -type f -iname '*.cc' -or -iname '*.cpp' -or -iname '*.c' | sed -r 's/^\.\//\t\t/' | \ + grep -E 'schur_eliminator_[0-9]_[0-9d]_[0-9d].cc|partitioned_matrix_view_[0-9]_[0-9d]_[0-9d].cc' | sort -d` +headers=`find ./include ./internal -type f -iname '*.h' | sed -r 's/^\.\//\t/' | sort -d` + +src_dir=`find ./internal -type f -iname '*.cc' -exec dirname {} \; -or -iname '*.cpp' -exec dirname {} \; -or -iname '*.c' -exec dirname {} \; | sed -r 's/^\.\//\t/' | sort -d | uniq` +src="" +for x in $src_dir $src_third_dir; do + t="" + + if test `echo "$x" | grep -c glog ` -eq 1; then + continue; + fi + + if test `echo "$x" | grep -c generated` -eq 1; then + continue; + fi + + if stat $x/*.cpp > /dev/null 2>&1; then + t="src += env.Glob('`echo $x'/*.cpp'`')" + fi + + if stat $x/*.c > /dev/null 2>&1; then + if [ -z "$t" ]; then + t="src += env.Glob('`echo $x'/*.c'`')" + else + t="$t + env.Glob('`echo $x'/*.c'`')" + fi + fi + + if stat $x/*.cc > /dev/null 2>&1; then + if [ -z "$t" ]; then + t="src += env.Glob('`echo $x'/*.cc'`')" + else + t="$t + env.Glob('`echo $x'/*.cc'`')" + fi + fi + + if [ -z "$src" ]; then + src=$t + else + src=`echo "$src\n$t"` + fi +done + +cat > CMakeLists.txt << EOF +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +# +# The Original Code is Copyright (C) 2012, Blender Foundation +# All rights reserved. +# +# Contributor(s): Blender Foundation, +# Sergey Sharybin +# +# ***** END GPL LICENSE BLOCK ***** + +# NOTE: This file is automatically generated by bundle.sh script +# If you're doing changes in this file, please update template +# in that script too + +set(INC + . + include + internal + config + ../gflags/src +) + +set(INC_SYS + \${EIGEN3_INCLUDE_DIRS} +) + +set(SRC +${sources} + +${headers} +) + +if(WITH_LIBMV_SCHUR_SPECIALIZATIONS) + list(APPEND SRC +${generated_sources} + ) +else() + add_definitions(-DCERES_RESTRICT_SCHUR_SPECIALIZATION) +endif() + +if(WIN32) + list(APPEND INC + ../glog/src/windows + ) +else() + list(APPEND INC + ../glog/src + ) +endif() + +add_definitions(\${GFLAGS_DEFINES}) +add_definitions(\${GLOG_DEFINES}) +add_definitions(\${CERES_DEFINES}) + +add_definitions( + -DCERES_HAVE_PTHREAD + -DCERES_NO_SUITESPARSE + -DCERES_NO_CXSPARSE + -DCERES_NO_LAPACK + -DCERES_HAVE_RWLOCK +) + +if(WITH_OPENMP) + add_definitions( + -DCERES_USE_OPENMP + ) +endif() + +TEST_UNORDERED_MAP_SUPPORT() +if(HAVE_STD_UNORDERED_MAP_HEADER) + if(HAVE_UNORDERED_MAP_IN_STD_NAMESPACE) + add_definitions(-DCERES_STD_UNORDERED_MAP) + else() + if(HAVE_UNORDERED_MAP_IN_TR1_NAMESPACE) + add_definitions(-DCERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE) + else() + add_definitions(-DCERES_NO_UNORDERED_MAP) + message(STATUS "Replacing unordered_map/set with map/set (warning: slower!)") + endif() + endif() +else() + if(HAVE_UNORDERED_MAP_IN_TR1_NAMESPACE) + add_definitions(-DCERES_TR1_UNORDERED_MAP) + else() + add_definitions(-DCERES_NO_UNORDERED_MAP) + message(STATUS "Replacing unordered_map/set with map/set (warning: slower!)") + endif() +endif() + +blender_add_lib(extern_ceres "\${SRC}" "\${INC}" "\${INC_SYS}") +EOF diff --git a/extern/ceres/config/ceres/internal/config.h b/extern/ceres/config/ceres/internal/config.h new file mode 100644 index 00000000000..1cf034ded5f --- /dev/null +++ b/extern/ceres/config/ceres/internal/config.h @@ -0,0 +1,48 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: alexs.mac@gmail.com (Alex Stewart) + +// Default (empty) configuration options for Ceres. +// +// IMPORTANT: Most users of Ceres will not use this file, when +// compiling Ceres with CMake, CMake will configure a new +// config.h with the currently selected Ceres compile +// options in <BUILD_DIR>/config, which will be added to +// the include path for compilation, and installed with the +// public Ceres headers. However, for some users of Ceres +// who compile without CMake (Android), this file ensures +// that Ceres will compile, with the user either specifying +// manually the Ceres compile options, or passing them +// directly through the compiler. + +#ifndef CERES_PUBLIC_INTERNAL_CONFIG_H_ +#define CERES_PUBLIC_INTERNAL_CONFIG_H_ + + +#endif // CERES_PUBLIC_INTERNAL_CONFIG_H_ diff --git a/extern/ceres/files.txt b/extern/ceres/files.txt new file mode 100644 index 00000000000..f49f1fb0ded --- /dev/null +++ b/extern/ceres/files.txt @@ -0,0 +1,252 @@ +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/dynamic_cost_function_to_functor.h +include/ceres/dynamic_numeric_diff_cost_function.h +include/ceres/fpclassify.h +include/ceres/gradient_checker.h +include/ceres/gradient_problem.h +include/ceres/gradient_problem_solver.h +include/ceres/internal/autodiff.h +include/ceres/internal/disable_warnings.h +include/ceres/internal/eigen.h +include/ceres/internal/fixed_array.h +include/ceres/internal/macros.h +include/ceres/internal/manual_constructor.h +include/ceres/internal/numeric_diff.h +include/ceres/internal/port.h +include/ceres/internal/reenable_warnings.h +include/ceres/internal/scoped_ptr.h +include/ceres/internal/variadic_evaluate.h +include/ceres/iteration_callback.h +include/ceres/jet.h +include/ceres/local_parameterization.h +include/ceres/loss_function.h +include/ceres/normal_prior.h +include/ceres/numeric_diff_cost_function.h +include/ceres/numeric_diff_options.h +include/ceres/ordered_groups.h +include/ceres/problem.h +include/ceres/rotation.h +include/ceres/sized_cost_function.h +include/ceres/solver.h +include/ceres/types.h +include/ceres/version.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 +internal/ceres/block_jacobian_writer.cc +internal/ceres/block_jacobian_writer.h +internal/ceres/block_jacobi_preconditioner.cc +internal/ceres/block_jacobi_preconditioner.h +internal/ceres/block_random_access_dense_matrix.cc +internal/ceres/block_random_access_dense_matrix.h +internal/ceres/block_random_access_diagonal_matrix.cc +internal/ceres/block_random_access_diagonal_matrix.h +internal/ceres/block_random_access_matrix.cc +internal/ceres/block_random_access_matrix.h +internal/ceres/block_random_access_sparse_matrix.cc +internal/ceres/block_random_access_sparse_matrix.h +internal/ceres/block_sparse_matrix.cc +internal/ceres/block_sparse_matrix.h +internal/ceres/block_structure.cc +internal/ceres/block_structure.h +internal/ceres/callbacks.cc +internal/ceres/callbacks.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 +internal/ceres/compressed_row_sparse_matrix.h +internal/ceres/conditioned_cost_function.cc +internal/ceres/conjugate_gradients_solver.cc +internal/ceres/conjugate_gradients_solver.h +internal/ceres/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.h +internal/ceres/dense_jacobian_writer.h +internal/ceres/dense_normal_cholesky_solver.cc +internal/ceres/dense_normal_cholesky_solver.h +internal/ceres/dense_qr_solver.cc +internal/ceres/dense_qr_solver.h +internal/ceres/dense_sparse_matrix.cc +internal/ceres/dense_sparse_matrix.h +internal/ceres/detect_structure.cc +internal/ceres/detect_structure.h +internal/ceres/dogleg_strategy.cc +internal/ceres/dogleg_strategy.h +internal/ceres/dynamic_compressed_row_finalizer.h +internal/ceres/dynamic_compressed_row_jacobian_writer.cc +internal/ceres/dynamic_compressed_row_jacobian_writer.h +internal/ceres/dynamic_compressed_row_sparse_matrix.cc +internal/ceres/dynamic_compressed_row_sparse_matrix.h +internal/ceres/evaluator.cc +internal/ceres/evaluator.h +internal/ceres/execution_summary.h +internal/ceres/file.cc +internal/ceres/file.h +internal/ceres/generated/partitioned_matrix_view_2_2_2.cc +internal/ceres/generated/partitioned_matrix_view_2_2_3.cc +internal/ceres/generated/partitioned_matrix_view_2_2_4.cc +internal/ceres/generated/partitioned_matrix_view_2_2_d.cc +internal/ceres/generated/partitioned_matrix_view_2_3_3.cc +internal/ceres/generated/partitioned_matrix_view_2_3_4.cc +internal/ceres/generated/partitioned_matrix_view_2_3_6.cc +internal/ceres/generated/partitioned_matrix_view_2_3_9.cc +internal/ceres/generated/partitioned_matrix_view_2_3_d.cc +internal/ceres/generated/partitioned_matrix_view_2_4_3.cc +internal/ceres/generated/partitioned_matrix_view_2_4_4.cc +internal/ceres/generated/partitioned_matrix_view_2_4_8.cc +internal/ceres/generated/partitioned_matrix_view_2_4_9.cc +internal/ceres/generated/partitioned_matrix_view_2_4_d.cc +internal/ceres/generated/partitioned_matrix_view_2_d_d.cc +internal/ceres/generated/partitioned_matrix_view_4_4_2.cc +internal/ceres/generated/partitioned_matrix_view_4_4_3.cc +internal/ceres/generated/partitioned_matrix_view_4_4_4.cc +internal/ceres/generated/partitioned_matrix_view_4_4_d.cc +internal/ceres/generated/partitioned_matrix_view_d_d_d.cc +internal/ceres/generated/schur_eliminator_2_2_2.cc +internal/ceres/generated/schur_eliminator_2_2_3.cc +internal/ceres/generated/schur_eliminator_2_2_4.cc +internal/ceres/generated/schur_eliminator_2_2_d.cc +internal/ceres/generated/schur_eliminator_2_3_3.cc +internal/ceres/generated/schur_eliminator_2_3_4.cc +internal/ceres/generated/schur_eliminator_2_3_6.cc +internal/ceres/generated/schur_eliminator_2_3_9.cc +internal/ceres/generated/schur_eliminator_2_3_d.cc +internal/ceres/generated/schur_eliminator_2_4_3.cc +internal/ceres/generated/schur_eliminator_2_4_4.cc +internal/ceres/generated/schur_eliminator_2_4_8.cc +internal/ceres/generated/schur_eliminator_2_4_9.cc +internal/ceres/generated/schur_eliminator_2_4_d.cc +internal/ceres/generated/schur_eliminator_2_d_d.cc +internal/ceres/generated/schur_eliminator_4_4_2.cc +internal/ceres/generated/schur_eliminator_4_4_3.cc +internal/ceres/generated/schur_eliminator_4_4_4.cc +internal/ceres/generated/schur_eliminator_4_4_d.cc +internal/ceres/generated/schur_eliminator_d_d_d.cc +internal/ceres/generate_eliminator_specialization.py +internal/ceres/generate_partitioned_matrix_view_specializations.py +internal/ceres/gradient_checking_cost_function.cc +internal/ceres/gradient_checking_cost_function.h +internal/ceres/gradient_problem.cc +internal/ceres/gradient_problem_evaluator.h +internal/ceres/gradient_problem_solver.cc +internal/ceres/graph_algorithms.h +internal/ceres/graph.h +internal/ceres/householder_vector.h +internal/ceres/implicit_schur_complement.cc +internal/ceres/implicit_schur_complement.h +internal/ceres/integral_types.h +internal/ceres/iterative_schur_complement_solver.cc +internal/ceres/iterative_schur_complement_solver.h +internal/ceres/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 +internal/ceres/linear_least_squares_problems.h +internal/ceres/linear_operator.cc +internal/ceres/linear_operator.h +internal/ceres/linear_solver.cc +internal/ceres/linear_solver.h +internal/ceres/line_search.cc +internal/ceres/line_search_direction.cc +internal/ceres/line_search_direction.h +internal/ceres/line_search.h +internal/ceres/line_search_minimizer.cc +internal/ceres/line_search_minimizer.h +internal/ceres/line_search_preprocessor.cc +internal/ceres/line_search_preprocessor.h +internal/ceres/local_parameterization.cc +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/minimizer.cc +internal/ceres/minimizer.h +internal/ceres/mutex.h +internal/ceres/normal_prior.cc +internal/ceres/parameter_block.h +internal/ceres/parameter_block_ordering.cc +internal/ceres/parameter_block_ordering.h +internal/ceres/partitioned_matrix_view.cc +internal/ceres/partitioned_matrix_view.h +internal/ceres/partitioned_matrix_view_impl.h +internal/ceres/polynomial.cc +internal/ceres/polynomial.h +internal/ceres/preconditioner.cc +internal/ceres/preconditioner.h +internal/ceres/preprocessor.cc +internal/ceres/preprocessor.h +internal/ceres/problem.cc +internal/ceres/problem_impl.cc +internal/ceres/problem_impl.h +internal/ceres/program.cc +internal/ceres/program_evaluator.h +internal/ceres/program.h +internal/ceres/random.h +internal/ceres/reorder_program.cc +internal/ceres/reorder_program.h +internal/ceres/residual_block.cc +internal/ceres/residual_block.h +internal/ceres/residual_block_utils.cc +internal/ceres/residual_block_utils.h +internal/ceres/schur_complement_solver.cc +internal/ceres/schur_complement_solver.h +internal/ceres/schur_eliminator.cc +internal/ceres/schur_eliminator.h +internal/ceres/schur_eliminator_impl.h +internal/ceres/schur_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_utils.cc +internal/ceres/solver_utils.h +internal/ceres/sparse_matrix.cc +internal/ceres/sparse_matrix.h +internal/ceres/sparse_normal_cholesky_solver.cc +internal/ceres/sparse_normal_cholesky_solver.h +internal/ceres/split.cc +internal/ceres/split.h +internal/ceres/stl_util.h +internal/ceres/stringprintf.cc +internal/ceres/stringprintf.h +internal/ceres/suitesparse.h +internal/ceres/triplet_sparse_matrix.cc +internal/ceres/triplet_sparse_matrix.h +internal/ceres/trust_region_minimizer.cc +internal/ceres/trust_region_minimizer.h +internal/ceres/trust_region_preprocessor.cc +internal/ceres/trust_region_preprocessor.h +internal/ceres/trust_region_strategy.cc +internal/ceres/trust_region_strategy.h +internal/ceres/types.cc +internal/ceres/visibility_based_preconditioner.h +internal/ceres/wall_time.cc +internal/ceres/wall_time.h +config/ceres/internal/config.h diff --git a/extern/ceres/include/ceres/autodiff_cost_function.h b/extern/ceres/include/ceres/autodiff_cost_function.h new file mode 100644 index 00000000000..e7893e4828e --- /dev/null +++ b/extern/ceres/include/ceres/autodiff_cost_function.h @@ -0,0 +1,227 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// Create CostFunctions as needed by the least squares framework, with +// Jacobians computed via automatic differentiation. For more +// information on automatic differentation, see the wikipedia article +// at http://en.wikipedia.org/wiki/Automatic_differentiation +// +// To get an auto differentiated cost function, you must define a class with a +// templated operator() (a functor) that computes the cost function in terms of +// the template parameter T. The autodiff framework substitutes appropriate +// "jet" objects for T in order to compute the derivative when necessary, but +// this is hidden, and you should write the function as if T were a scalar type +// (e.g. a double-precision floating point number). +// +// The function must write the computed value in the last argument +// (the only non-const one) and return true to indicate +// success. 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 +// transposition, and k is a constant. The form of this error, which is the +// difference between a constant and an expression, is a common pattern in least +// squares problems. For example, the value x'y might be the model expectation +// for a series of measurements, where there is an instance of the cost function +// for each measurement k. +// +// The actual cost added to the total problem is e^2, or (k - x'k)^2; however, +// the squaring is implicitly done by the optimization framework. +// +// To write an auto-differentiable cost function for the above model, first +// define the object +// +// class MyScalarCostFunctor { +// MyScalarCostFunctor(double k): k_(k) {} +// +// template <typename T> +// bool operator()(const T* const x , const T* const y, T* e) const { +// e[0] = T(k_) - x[0] * y[0] + x[1] * y[1]; +// return true; +// } +// +// private: +// double k_; +// }; +// +// Note that in the declaration of operator() the input parameters x and y come +// first, and are passed as const pointers to arrays of T. If there were three +// input parameters, then the third input parameter would come after y. The +// output is always the last parameter, and is also a pointer to an array. In +// the example above, e is a scalar, so only e[0] is set. +// +// Then given this class definition, the auto differentiated cost function for +// it can be constructed as follows. +// +// CostFunction* cost_function +// = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>( +// new MyScalarCostFunctor(1.0)); ^ ^ ^ +// | | | +// Dimension of residual -----+ | | +// Dimension of x ---------------+ | +// Dimension of y ------------------+ +// +// In this example, there is usually an instance for each measumerent of k. +// +// In the instantiation above, the template parameters following +// "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing a +// 1-dimensional output from two arguments, both 2-dimensional. +// +// AutoDiffCostFunction also supports cost functions with a +// runtime-determined number of residuals. For example: +// +// CostFunction* cost_function +// = new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>( +// new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^ +// runtime_number_of_residuals); <----+ | | | +// | | | | +// | | | | +// Actual number of residuals ------+ | | | +// Indicate dynamic number of residuals --------+ | | +// Dimension of x ------------------------------------+ | +// Dimension of y ---------------------------------------+ +// +// The framework can currently accommodate cost functions of up to 10 +// independent variables, and there is no limit on the dimensionality +// of each of them. +// +// WARNING #1: Since the functor will get instantiated with different types for +// T, you must to convert from other numeric types to T before mixing +// computations with other variables of type T. In the example above, this is +// seen where instead of using k_ directly, k_ is wrapped with T(k_). +// +// WARNING #2: A common beginner's error when first using autodiff cost +// functions is to get the sizing wrong. In particular, there is a tendency to +// set the template parameters to (dimension of residual, number of parameters) +// instead of passing a dimension parameter for *every parameter*. In the +// example above, that would be <MyScalarCostFunctor, 1, 2>, which is missing +// the last '2' argument. Please be careful when setting the size parameters. + +#ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ + +#include "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 { + +// A cost function which computes the derivative of the cost with respect to +// the parameters (a.k.a. the jacobian) using an autodifferentiation framework. +// The first template argument is the functor object, described in the header +// comment. The second argument is the dimension of the residual (or +// ceres::DYNAMIC to indicate it will be set at runtime), and subsequent +// arguments describe the size of the Nth parameter, one per parameter. +// +// The constructors take ownership of the cost functor. +// +// If the number of residuals (argument kNumResiduals below) is +// ceres::DYNAMIC, then the two-argument constructor must be used. The +// second constructor takes a number of residuals (in addition to the +// templated number of residuals). This allows for varying the number +// of residuals for a single autodiff cost function at runtime. +template <typename CostFunctor, + int kNumResiduals, // Number of residuals, or ceres::DYNAMIC. + int N0, // Number of parameters in block 0. + int N1 = 0, // Number of parameters in block 1. + int N2 = 0, // Number of parameters in block 2. + int N3 = 0, // Number of parameters in block 3. + int N4 = 0, // Number of parameters in block 4. + int N5 = 0, // Number of parameters in block 5. + int N6 = 0, // Number of parameters in block 6. + int N7 = 0, // Number of parameters in block 7. + int N8 = 0, // Number of parameters in block 8. + int N9 = 0> // Number of parameters in block 9. +class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, + N0, N1, N2, N3, N4, + N5, N6, N7, N8, N9> { + public: + // Takes ownership of functor. Uses the template-provided value for the + // number of residuals ("kNumResiduals"). + explicit AutoDiffCostFunction(CostFunctor* functor) + : functor_(functor) { + CHECK_NE(kNumResiduals, DYNAMIC) + << "Can't run the fixed-size constructor if the " + << "number of residuals is set to ceres::DYNAMIC."; + } + + // Takes ownership of functor. Ignores the template-provided + // kNumResiduals in favor of the "num_residuals" argument provided. + // + // This allows for having autodiff cost functions which return varying + // numbers of residuals at runtime. + AutoDiffCostFunction(CostFunctor* functor, int num_residuals) + : functor_(functor) { + CHECK_EQ(kNumResiduals, DYNAMIC) + << "Can't run the dynamic-size constructor if the " + << "number of residuals is not ceres::DYNAMIC."; + SizedCostFunction<kNumResiduals, + N0, N1, N2, N3, N4, + N5, N6, N7, N8, N9> + ::set_num_residuals(num_residuals); + } + + virtual ~AutoDiffCostFunction() {} + + // Implementation details follow; clients of the autodiff cost function should + // not have to examine below here. + // + // To handle varardic cost functions, some template magic is needed. It's + // mostly hidden inside autodiff.h. + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + if (!jacobians) { + return internal::VariadicEvaluate< + CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> + ::Call(*functor_, parameters, residuals); + } + return internal::AutoDiff<CostFunctor, double, + N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate( + *functor_, + parameters, + SizedCostFunction<kNumResiduals, + N0, N1, N2, N3, N4, + N5, N6, N7, N8, N9>::num_residuals(), + residuals, + jacobians); + } + + private: + internal::scoped_ptr<CostFunctor> functor_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/autodiff_local_parameterization.h b/extern/ceres/include/ceres/autodiff_local_parameterization.h new file mode 100644 index 00000000000..27397e20d3b --- /dev/null +++ b/extern/ceres/include/ceres/autodiff_local_parameterization.h @@ -0,0 +1,154 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: sergey.vfx@gmail.com (Sergey Sharybin) +// mierle@gmail.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ +#define CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ + +#include "ceres/local_parameterization.h" +#include "ceres/internal/autodiff.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { + +// Create local parameterization with Jacobians computed via automatic +// differentiation. For more information on local parameterizations, +// see include/ceres/local_parameterization.h +// +// To get an auto differentiated local parameterization, you must define +// a class with a templated operator() (a functor) that computes +// +// x_plus_delta = Plus(x, delta); +// +// the template parameter T. The autodiff framework substitutes appropriate +// "Jet" objects for T in order to compute the derivative when necessary, but +// this is hidden, and you should write the function as if T were a scalar type +// (e.g. a double-precision floating point number). +// +// The function must write the computed value in the last argument (the only +// non-const one) and return true to indicate success. +// +// For example, Quaternions have a three dimensional local +// parameterization. It's plus operation can be implemented as (taken +// from internal/ceres/auto_diff_local_parameterization_test.cc) +// +// struct QuaternionPlus { +// template<typename T> +// bool operator()(const T* x, const T* delta, T* x_plus_delta) const { +// const T squared_norm_delta = +// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; +// +// T q_delta[4]; +// if (squared_norm_delta > T(0.0)) { +// T norm_delta = sqrt(squared_norm_delta); +// const T sin_delta_by_delta = sin(norm_delta) / norm_delta; +// q_delta[0] = cos(norm_delta); +// q_delta[1] = sin_delta_by_delta * delta[0]; +// q_delta[2] = sin_delta_by_delta * delta[1]; +// q_delta[3] = sin_delta_by_delta * delta[2]; +// } else { +// // We do not just use q_delta = [1,0,0,0] here because that is a +// // constant and when used for automatic differentiation will +// // lead to a zero derivative. Instead we take a first order +// // approximation and evaluate it at zero. +// q_delta[0] = T(1.0); +// q_delta[1] = delta[0]; +// q_delta[2] = delta[1]; +// q_delta[3] = delta[2]; +// } +// +// QuaternionProduct(q_delta, x, x_plus_delta); +// return true; +// } +// }; +// +// Then given this struct, the auto differentiated local +// parameterization can now be constructed as +// +// LocalParameterization* local_parameterization = +// new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>; +// | | +// Global Size ---------------+ | +// Local Size -------------------+ +// +// WARNING: Since the functor will get instantiated with different types for +// T, you must to convert from other numeric types to T before mixing +// computations with other variables of type T. In the example above, this is +// seen where instead of using k_ directly, k_ is wrapped with T(k_). + +template <typename Functor, int kGlobalSize, int kLocalSize> +class AutoDiffLocalParameterization : public LocalParameterization { + public: + AutoDiffLocalParameterization() : + functor_(new Functor()) {} + + // Takes ownership of functor. + explicit AutoDiffLocalParameterization(Functor* functor) : + functor_(functor) {} + + virtual ~AutoDiffLocalParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + return (*functor_)(x, delta, x_plus_delta); + } + + virtual bool ComputeJacobian(const double* x, double* jacobian) const { + double zero_delta[kLocalSize]; + for (int i = 0; i < kLocalSize; ++i) { + zero_delta[i] = 0.0; + } + + double x_plus_delta[kGlobalSize]; + for (int i = 0; i < kGlobalSize; ++i) { + x_plus_delta[i] = 0.0; + } + + const double* parameter_ptrs[2] = {x, zero_delta}; + double* jacobian_ptrs[2] = { NULL, jacobian }; + return internal::AutoDiff<Functor, double, kGlobalSize, kLocalSize> + ::Differentiate(*functor_, + parameter_ptrs, + kGlobalSize, + x_plus_delta, + jacobian_ptrs); + } + + virtual int GlobalSize() const { return kGlobalSize; } + virtual int LocalSize() const { return kLocalSize; } + + private: + internal::scoped_ptr<Functor> functor_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ diff --git a/extern/ceres/include/ceres/c_api.h b/extern/ceres/include/ceres/c_api.h new file mode 100644 index 00000000000..df7c9b6d671 --- /dev/null +++ b/extern/ceres/include/ceres/c_api.h @@ -0,0 +1,146 @@ +/* Ceres Solver - A fast non-linear least squares minimizer + * Copyright 2015 Google Inc. All rights reserved. + * http://ceres-solver.org/ + * + * 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_ + +#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* Init the Ceres private data. Must be called before anything else. */ +CERES_EXPORT 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. + */ +CERES_EXPORT void* ceres_create_huber_loss_function_data(double a); +CERES_EXPORT void* ceres_create_softl1_loss_function_data(double a); +CERES_EXPORT void* ceres_create_cauchy_loss_function_data(double a); +CERES_EXPORT void* ceres_create_arctan_loss_function_data(double a); +CERES_EXPORT void* ceres_create_tolerant_loss_function_data(double a, double b); + +/* Free the given stock loss function data. */ +CERES_EXPORT 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. */ +CERES_EXPORT 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_EXPORT ceres_problem_t* ceres_create_problem(); +CERES_EXPORT void ceres_free_problem(ceres_problem_t* problem); + +/* Add a residual block. */ +CERES_EXPORT 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); + +CERES_EXPORT void ceres_solve(ceres_problem_t* problem); + +/* TODO(keir): Figure out a way to pass a config in. */ + +#ifdef __cplusplus +} +#endif + +#include "ceres/internal/reenable_warnings.h" + +#endif /* CERES_PUBLIC_C_API_H_ */ diff --git a/extern/ceres/include/ceres/ceres.h b/extern/ceres/include/ceres/ceres.h new file mode 100644 index 00000000000..64ffb99798a --- /dev/null +++ b/extern/ceres/include/ceres/ceres.h @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// This is a forwarding header containing the public symbols exported from +// Ceres. Anything in the "ceres" namespace is available for use. + +#ifndef CERES_PUBLIC_CERES_H_ +#define CERES_PUBLIC_CERES_H_ + +#include "ceres/autodiff_cost_function.h" +#include "ceres/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/dynamic_autodiff_cost_function.h" +#include "ceres/dynamic_numeric_diff_cost_function.h" +#include "ceres/gradient_problem.h" +#include "ceres/gradient_problem_solver.h" +#include "ceres/iteration_callback.h" +#include "ceres/jet.h" +#include "ceres/local_parameterization.h" +#include "ceres/loss_function.h" +#include "ceres/numeric_diff_cost_function.h" +#include "ceres/ordered_groups.h" +#include "ceres/problem.h" +#include "ceres/sized_cost_function.h" +#include "ceres/solver.h" +#include "ceres/types.h" +#include "ceres/version.h" + +#endif // CERES_PUBLIC_CERES_H_ diff --git a/extern/ceres/include/ceres/conditioned_cost_function.h b/extern/ceres/include/ceres/conditioned_cost_function.h new file mode 100644 index 00000000000..29597d935cb --- /dev/null +++ b/extern/ceres/include/ceres/conditioned_cost_function.h @@ -0,0 +1,99 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: wjr@google.com (William Rucklidge) +// +// This file contains a cost function that can apply a transformation to +// each residual value before they are square-summed. + +#ifndef CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ +#define CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ + +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" +#include "ceres/internal/disable_warnings.h" + +namespace ceres { + +// This class allows you to apply different conditioning to the residual +// values of a wrapped cost function. An example where this is useful is +// where you have an existing cost function that produces N values, but you +// want the total cost to be something other than just the sum of these +// squared values - maybe you want to apply a different scaling to some +// values, to change their contribution to the cost. +// +// Usage: +// +// // my_cost_function produces N residuals +// CostFunction* my_cost_function = ... +// CHECK_EQ(N, my_cost_function->num_residuals()); +// vector<CostFunction*> conditioners; +// +// // Make N 1x1 cost functions (1 parameter, 1 residual) +// CostFunction* f_1 = ... +// conditioners.push_back(f_1); +// ... +// CostFunction* f_N = ... +// conditioners.push_back(f_N); +// ConditionedCostFunction* ccf = +// new ConditionedCostFunction(my_cost_function, conditioners); +// +// Now ccf's residual i (i=0..N-1) will be passed though the i'th conditioner. +// +// ccf_residual[i] = f_i(my_cost_function_residual[i]) +// +// and the Jacobian will be affected appropriately. +class CERES_EXPORT ConditionedCostFunction : public CostFunction { + public: + // Builds a cost function based on a wrapped cost function, and a + // per-residual conditioner. Takes ownership of all of the wrapped cost + // functions, or not, depending on the ownership parameter. Conditioners + // may be NULL, in which case the corresponding residual is not modified. + ConditionedCostFunction(CostFunction* wrapped_cost_function, + const std::vector<CostFunction*>& conditioners, + Ownership ownership); + virtual ~ConditionedCostFunction(); + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const; + + private: + internal::scoped_ptr<CostFunction> wrapped_cost_function_; + std::vector<CostFunction*> conditioners_; + Ownership ownership_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/cost_function.h b/extern/ceres/include/ceres/cost_function.h new file mode 100644 index 00000000000..f051a897c0d --- /dev/null +++ b/extern/ceres/include/ceres/cost_function.h @@ -0,0 +1,147 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.m (Keir Mierle) +// +// This is the interface through which the least squares solver accesses the +// residual and Jacobian of the least squares problem. Users are expected to +// subclass CostFunction to define their own terms in the least squares problem. +// +// It is recommended that users define templated residual functors for use as +// arguments for AutoDiffCostFunction (see autodiff_cost_function.h), instead of +// directly implementing the CostFunction interface. This often results in both +// shorter code and faster execution than hand-coded derivatives. However, +// specialized cases may demand direct implementation of the lower-level +// CostFunction interface; for example, this is true when calling legacy code +// which is not templated on numeric types. + +#ifndef CERES_PUBLIC_COST_FUNCTION_H_ +#define CERES_PUBLIC_COST_FUNCTION_H_ + +#include <vector> +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/types.h" +#include "ceres/internal/disable_warnings.h" + +namespace ceres { + +// This class implements the computation of the cost (a.k.a. residual) terms as +// a function of the input (control) variables, and is the interface for users +// to describe their least squares problem to Ceres. In other words, this is the +// modelling layer between users and the Ceres optimizer. The signature of the +// function (number and sizes of input parameter blocks and number of outputs) +// is stored in parameter_block_sizes_ and num_residuals_ respectively. User +// code inheriting from this class is expected to set these two members with the +// corresponding accessors. This information will be verified by the Problem +// when added with AddResidualBlock(). +class CERES_EXPORT CostFunction { + public: + CostFunction() : num_residuals_(0) {} + + virtual ~CostFunction() {} + + // Inputs: + // + // parameters is an array of pointers to arrays containing the + // various parameter blocks. parameters has the same number of + // elements as parameter_block_sizes_. Parameter blocks are in the + // same order as parameter_block_sizes_.i.e., + // + // parameters_[i] = double[parameter_block_sizes_[i]] + // + // Outputs: + // + // residuals is an array of size num_residuals_. + // + // jacobians is an array of size parameter_block_sizes_ containing + // pointers to storage for jacobian blocks corresponding to each + // parameter block. Jacobian blocks are in the same order as + // parameter_block_sizes, i.e. jacobians[i], is an + // array that contains num_residuals_* parameter_block_sizes_[i] + // elements. Each jacobian block is stored in row-major order, i.e., + // + // jacobians[i][r*parameter_block_size_[i] + c] = + // d residual[r] / d parameters[i][c] + // + // If jacobians is NULL, then no derivatives are returned; this is + // the case when computing cost only. If jacobians[i] is NULL, then + // the jacobian block corresponding to the i'th parameter block must + // not to be returned. + // + // 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. + // + // 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; + + const std::vector<int32>& parameter_block_sizes() const { + return parameter_block_sizes_; + } + + int num_residuals() const { + return num_residuals_; + } + + protected: + std::vector<int32>* mutable_parameter_block_sizes() { + return ¶meter_block_sizes_; + } + + void set_num_residuals(int num_residuals) { + num_residuals_ = num_residuals; + } + + private: + // Cost function signature metadata: number of inputs & their sizes, + // number of outputs (residuals). + std::vector<int32> parameter_block_sizes_; + int num_residuals_; + CERES_DISALLOW_COPY_AND_ASSIGN(CostFunction); +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_COST_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/cost_function_to_functor.h b/extern/ceres/include/ceres/cost_function_to_functor.h new file mode 100644 index 00000000000..6c67ac0f937 --- /dev/null +++ b/extern/ceres/include/ceres/cost_function_to_functor.h @@ -0,0 +1,682 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// CostFunctionToFunctor is an adapter class that allows users to use +// SizedCostFunction objects in templated functors which are to be used for +// automatic differentiation. This allows the user to seamlessly mix +// analytic, numeric and automatic differentiation. +// +// For example, let us assume that +// +// class IntrinsicProjection : public SizedCostFunction<2, 5, 3> { +// public: +// IntrinsicProjection(const double* observation); +// virtual bool Evaluate(double const* const* parameters, +// double* residuals, +// double** jacobians) const; +// }; +// +// is a cost function that implements the projection of a point in its +// local coordinate system onto its image plane and subtracts it from +// the observed point projection. It can compute its residual and +// either via analytic or numerical differentiation can compute its +// jacobians. +// +// Now we would like to compose the action of this CostFunction with +// the action of camera extrinsics, i.e., rotation and +// translation. Say we have a templated function +// +// template<typename T> +// void RotateAndTranslatePoint(const T* rotation, +// const T* translation, +// const T* point, +// T* result); +// +// Then we can now do the following, +// +// struct CameraProjection { +// CameraProjection(const double* observation) +// : intrinsic_projection_(new IntrinsicProjection(observation)) { +// } +// template <typename T> +// bool operator()(const T* rotation, +// const T* translation, +// const T* intrinsics, +// const T* point, +// T* residual) const { +// T transformed_point[3]; +// RotateAndTranslatePoint(rotation, translation, point, transformed_point); +// +// // Note that we call intrinsic_projection_, just like it was +// // any other templated functor. +// +// return intrinsic_projection_(intrinsics, transformed_point, residual); +// } +// +// private: +// CostFunctionToFunctor<2,5,3> intrinsic_projection_; +// }; + +#ifndef CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_ +#define CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_ + +#include <numeric> +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/dynamic_cost_function_to_functor.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { + +template <int kNumResiduals, + int N0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, + int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> +class CostFunctionToFunctor { + public: + // Takes ownership of cost_function. + explicit CostFunctionToFunctor(CostFunction* cost_function) + : cost_functor_(cost_function) { + CHECK_NOTNULL(cost_function); + CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC); + + // This block breaks the 80 column rule to keep it somewhat readable. + CHECK((!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) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT + << "Zero block cannot precede a non-zero block. Block sizes are " + << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " + << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " + << N8 << ", " << N9; + + const std::vector<int32>& parameter_block_sizes = + cost_function->parameter_block_sizes(); + const int num_parameter_blocks = + (N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) + + (N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0); + CHECK_EQ(parameter_block_sizes.size(), num_parameter_blocks); + + CHECK_EQ(N0, parameter_block_sizes[0]); + if (parameter_block_sizes.size() > 1) CHECK_EQ(N1, parameter_block_sizes[1]); // NOLINT + if (parameter_block_sizes.size() > 2) CHECK_EQ(N2, parameter_block_sizes[2]); // NOLINT + if (parameter_block_sizes.size() > 3) CHECK_EQ(N3, parameter_block_sizes[3]); // NOLINT + if (parameter_block_sizes.size() > 4) CHECK_EQ(N4, parameter_block_sizes[4]); // NOLINT + if (parameter_block_sizes.size() > 5) CHECK_EQ(N5, parameter_block_sizes[5]); // NOLINT + if (parameter_block_sizes.size() > 6) CHECK_EQ(N6, parameter_block_sizes[6]); // NOLINT + if (parameter_block_sizes.size() > 7) CHECK_EQ(N7, parameter_block_sizes[7]); // NOLINT + if (parameter_block_sizes.size() > 8) CHECK_EQ(N8, parameter_block_sizes[8]); // NOLINT + if (parameter_block_sizes.size() > 9) CHECK_EQ(N9, parameter_block_sizes[9]); // NOLINT + + CHECK_EQ(accumulate(parameter_block_sizes.begin(), + parameter_block_sizes.end(), 0), + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9); + } + + bool operator()(const double* x0, double* residuals) const { + CHECK_NE(N0, 0); + CHECK_EQ(N1, 0); + CHECK_EQ(N2, 0); + CHECK_EQ(N3, 0); + CHECK_EQ(N4, 0); + CHECK_EQ(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + + return cost_functor_(&x0, residuals); + } + + bool operator()(const double* x0, + const double* x1, + double* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_EQ(N2, 0); + CHECK_EQ(N3, 0); + CHECK_EQ(N4, 0); + CHECK_EQ(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const double*> parameter_blocks(2); + parameter_blocks[0] = x0; + parameter_blocks[1] = x1; + return cost_functor_(parameter_blocks.get(), residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + double* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_EQ(N3, 0); + CHECK_EQ(N4, 0); + CHECK_EQ(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const double*> parameter_blocks(3); + parameter_blocks[0] = x0; + parameter_blocks[1] = x1; + parameter_blocks[2] = x2; + return cost_functor_(parameter_blocks.get(), residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + double* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_EQ(N4, 0); + CHECK_EQ(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const double*> parameter_blocks(4); + parameter_blocks[0] = x0; + parameter_blocks[1] = x1; + parameter_blocks[2] = x2; + parameter_blocks[3] = x3; + return cost_functor_(parameter_blocks.get(), residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + double* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_EQ(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const double*> parameter_blocks(5); + parameter_blocks[0] = x0; + parameter_blocks[1] = x1; + parameter_blocks[2] = x2; + parameter_blocks[3] = x3; + parameter_blocks[4] = x4; + return cost_functor_(parameter_blocks.get(), residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + const double* x5, + double* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_NE(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const double*> parameter_blocks(6); + parameter_blocks[0] = x0; + parameter_blocks[1] = x1; + parameter_blocks[2] = x2; + parameter_blocks[3] = x3; + parameter_blocks[4] = x4; + parameter_blocks[5] = x5; + return cost_functor_(parameter_blocks.get(), residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + const double* x5, + const double* x6, + double* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_NE(N5, 0); + CHECK_NE(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const double*> parameter_blocks(7); + parameter_blocks[0] = x0; + parameter_blocks[1] = x1; + parameter_blocks[2] = x2; + parameter_blocks[3] = x3; + parameter_blocks[4] = x4; + parameter_blocks[5] = x5; + parameter_blocks[6] = x6; + return cost_functor_(parameter_blocks.get(), residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + const double* x5, + const double* x6, + const double* x7, + double* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_NE(N5, 0); + CHECK_NE(N6, 0); + CHECK_NE(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const double*> parameter_blocks(8); + parameter_blocks[0] = x0; + parameter_blocks[1] = x1; + parameter_blocks[2] = x2; + parameter_blocks[3] = x3; + parameter_blocks[4] = x4; + parameter_blocks[5] = x5; + parameter_blocks[6] = x6; + parameter_blocks[7] = x7; + return cost_functor_(parameter_blocks.get(), residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + const double* x5, + const double* x6, + const double* x7, + const double* x8, + double* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_NE(N5, 0); + CHECK_NE(N6, 0); + CHECK_NE(N7, 0); + CHECK_NE(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const double*> parameter_blocks(9); + parameter_blocks[0] = x0; + parameter_blocks[1] = x1; + parameter_blocks[2] = x2; + parameter_blocks[3] = x3; + parameter_blocks[4] = x4; + parameter_blocks[5] = x5; + parameter_blocks[6] = x6; + parameter_blocks[7] = x7; + parameter_blocks[8] = x8; + return cost_functor_(parameter_blocks.get(), residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + const double* x5, + const double* x6, + const double* x7, + const double* x8, + const double* x9, + double* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_NE(N5, 0); + CHECK_NE(N6, 0); + CHECK_NE(N7, 0); + CHECK_NE(N8, 0); + CHECK_NE(N9, 0); + internal::FixedArray<const double*> parameter_blocks(10); + parameter_blocks[0] = x0; + parameter_blocks[1] = x1; + parameter_blocks[2] = x2; + parameter_blocks[3] = x3; + parameter_blocks[4] = x4; + parameter_blocks[5] = x5; + parameter_blocks[6] = x6; + parameter_blocks[7] = x7; + parameter_blocks[8] = x8; + parameter_blocks[9] = x9; + return cost_functor_(parameter_blocks.get(), residuals); + } + + template <typename JetT> + bool operator()(const JetT* x0, JetT* residuals) const { + CHECK_NE(N0, 0); + CHECK_EQ(N1, 0); + CHECK_EQ(N2, 0); + CHECK_EQ(N3, 0); + CHECK_EQ(N4, 0); + CHECK_EQ(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + return cost_functor_(&x0, residuals); + } + + template <typename JetT> + bool operator()(const JetT* x0, + const JetT* x1, + JetT* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_EQ(N2, 0); + CHECK_EQ(N3, 0); + CHECK_EQ(N4, 0); + CHECK_EQ(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const JetT*> jets(2); + jets[0] = x0; + jets[1] = x1; + return cost_functor_(jets.get(), residuals); + } + + template <typename JetT> + bool operator()(const JetT* x0, + const JetT* x1, + const JetT* x2, + JetT* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_EQ(N3, 0); + CHECK_EQ(N4, 0); + CHECK_EQ(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const JetT*> jets(3); + jets[0] = x0; + jets[1] = x1; + jets[2] = x2; + return cost_functor_(jets.get(), residuals); + } + + template <typename JetT> + bool operator()(const JetT* x0, + const JetT* x1, + const JetT* x2, + const JetT* x3, + JetT* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_EQ(N4, 0); + CHECK_EQ(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const JetT*> jets(4); + jets[0] = x0; + jets[1] = x1; + jets[2] = x2; + jets[3] = x3; + return cost_functor_(jets.get(), residuals); + } + + template <typename JetT> + bool operator()(const JetT* x0, + const JetT* x1, + const JetT* x2, + const JetT* x3, + const JetT* x4, + JetT* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_EQ(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const JetT*> jets(5); + jets[0] = x0; + jets[1] = x1; + jets[2] = x2; + jets[3] = x3; + jets[4] = x4; + return cost_functor_(jets.get(), residuals); + } + + template <typename JetT> + bool operator()(const JetT* x0, + const JetT* x1, + const JetT* x2, + const JetT* x3, + const JetT* x4, + const JetT* x5, + JetT* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_NE(N5, 0); + CHECK_EQ(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const JetT*> jets(6); + jets[0] = x0; + jets[1] = x1; + jets[2] = x2; + jets[3] = x3; + jets[4] = x4; + jets[5] = x5; + return cost_functor_(jets.get(), residuals); + } + + template <typename JetT> + bool operator()(const JetT* x0, + const JetT* x1, + const JetT* x2, + const JetT* x3, + const JetT* x4, + const JetT* x5, + const JetT* x6, + JetT* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_NE(N5, 0); + CHECK_NE(N6, 0); + CHECK_EQ(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const JetT*> jets(7); + jets[0] = x0; + jets[1] = x1; + jets[2] = x2; + jets[3] = x3; + jets[4] = x4; + jets[5] = x5; + jets[6] = x6; + return cost_functor_(jets.get(), residuals); + } + + template <typename JetT> + bool operator()(const JetT* x0, + const JetT* x1, + const JetT* x2, + const JetT* x3, + const JetT* x4, + const JetT* x5, + const JetT* x6, + const JetT* x7, + JetT* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_NE(N5, 0); + CHECK_NE(N6, 0); + CHECK_NE(N7, 0); + CHECK_EQ(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const JetT*> jets(8); + jets[0] = x0; + jets[1] = x1; + jets[2] = x2; + jets[3] = x3; + jets[4] = x4; + jets[5] = x5; + jets[6] = x6; + jets[7] = x7; + return cost_functor_(jets.get(), residuals); + } + + template <typename JetT> + bool operator()(const JetT* x0, + const JetT* x1, + const JetT* x2, + const JetT* x3, + const JetT* x4, + const JetT* x5, + const JetT* x6, + const JetT* x7, + const JetT* x8, + JetT* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_NE(N5, 0); + CHECK_NE(N6, 0); + CHECK_NE(N7, 0); + CHECK_NE(N8, 0); + CHECK_EQ(N9, 0); + internal::FixedArray<const JetT*> jets(9); + jets[0] = x0; + jets[1] = x1; + jets[2] = x2; + jets[3] = x3; + jets[4] = x4; + jets[5] = x5; + jets[6] = x6; + jets[7] = x7; + jets[8] = x8; + return cost_functor_(jets.get(), residuals); + } + + template <typename JetT> + bool operator()(const JetT* x0, + const JetT* x1, + const JetT* x2, + const JetT* x3, + const JetT* x4, + const JetT* x5, + const JetT* x6, + const JetT* x7, + const JetT* x8, + const JetT* x9, + JetT* residuals) const { + CHECK_NE(N0, 0); + CHECK_NE(N1, 0); + CHECK_NE(N2, 0); + CHECK_NE(N3, 0); + CHECK_NE(N4, 0); + CHECK_NE(N5, 0); + CHECK_NE(N6, 0); + CHECK_NE(N7, 0); + CHECK_NE(N8, 0); + CHECK_NE(N9, 0); + internal::FixedArray<const JetT*> jets(10); + jets[0] = x0; + jets[1] = x1; + jets[2] = x2; + jets[3] = x3; + jets[4] = x4; + jets[5] = x5; + jets[6] = x6; + jets[7] = x7; + jets[8] = x8; + jets[9] = x9; + return cost_functor_(jets.get(), residuals); + } + + private: + DynamicCostFunctionToFunctor cost_functor_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_ diff --git a/extern/ceres/include/ceres/covariance.h b/extern/ceres/include/ceres/covariance.h new file mode 100644 index 00000000000..dd20dc36ba1 --- /dev/null +++ b/extern/ceres/include/ceres/covariance.h @@ -0,0 +1,405 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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" +#include "ceres/internal/disable_warnings.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); +// +// std::vector<std::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 CERES_EXPORT Covariance { + public: + struct CERES_EXPORT Options { + Options() +#ifndef CERES_NO_SUITESPARSE + : algorithm_type(SUITE_SPARSE_QR), +#else + : algorithm_type(EIGEN_SPARSE_QR), +#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. EIGEN_SPARSE_QR uses the sparse QR factorization algorithm + // in Eigen to compute the decomposition + // + // Q * R = J + // + // [J'J]^-1 = [R*R']^-1 + // + // It is a moderately fast algorithm for sparse matrices. + // + // 3. SUITE_SPARSE_QR uses the SuiteSparseQR sparse QR + // factorization algorithm. It uses dense linear algebra and is + // multi threaded, so for large sparse sparse matrices it is + // significantly faster than EIGEN_SPARSE_QR. + // + // Neither EIGEN_SPARSE_QR not SUITE_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. SUITE_SPARSE_QR and EIGEN_SPARSE_QR + // + // rank(J) < num_col(J) + // + // Here rank(J) is the estimate of the rank of J returned by the + // sparse QR factorization 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 SUITE_SPARSE_QR and + // EIGEN_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 std::vector<std::pair<const double*, + const double*> >& covariance_blocks, + Problem* problem); + + // Return the block of the cross-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; + + // Return the block of the cross-covariance matrix corresponding to + // parameter_block1 and parameter_block2. + // Returns cross-covariance in the tangent space if a local + // parameterization is associated with either parameter block; + // else returns cross-covariance in the ambient space. + // + // 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_local_size x parameter_block2_local_size matrix. The + // returned covariance will be a row-major matrix. + bool GetCovarianceBlockInTangentSpace(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const; + + private: + internal::scoped_ptr<internal::CovarianceImpl> impl_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_COVARIANCE_H_ diff --git a/extern/ceres/include/ceres/crs_matrix.h b/extern/ceres/include/ceres/crs_matrix.h new file mode 100644 index 00000000000..23687c4670e --- /dev/null +++ b/extern/ceres/include/ceres/crs_matrix.h @@ -0,0 +1,86 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_CRS_MATRIX_H_ +#define CERES_PUBLIC_CRS_MATRIX_H_ + +#include <vector> +#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" + +namespace ceres { + +// A compressed row sparse matrix used primarily for communicating the +// Jacobian matrix to the user. +struct CERES_EXPORT CRSMatrix { + CRSMatrix() : num_rows(0), num_cols(0) {} + + int num_rows; + int num_cols; + + // A compressed row matrix stores its contents in three arrays, + // rows, cols and values. + // + // rows is a num_rows + 1 sized array that points into the cols and + // values array. For each row i: + // + // cols[rows[i]] ... cols[rows[i + 1] - 1] are the indices of the + // non-zero columns of row i. + // + // values[rows[i]] .. values[rows[i + 1] - 1] are the values of the + // corresponding entries. + // + // cols and values contain as many entries as there are non-zeros in + // the matrix. + // + // e.g, consider the 3x4 sparse matrix + // + // [ 0 10 0 4 ] + // [ 0 2 -3 2 ] + // [ 1 2 0 0 ] + // + // The three arrays will be: + // + // + // -row0- ---row1--- -row2- + // rows = [ 0, 2, 5, 7] + // cols = [ 1, 3, 1, 2, 3, 0, 1] + // values = [10, 4, 2, -3, 2, 1, 2] + + std::vector<int> cols; + std::vector<int> rows; + std::vector<double> values; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_CRS_MATRIX_H_ diff --git a/extern/ceres/include/ceres/dynamic_autodiff_cost_function.h b/extern/ceres/include/ceres/dynamic_autodiff_cost_function.h new file mode 100644 index 00000000000..e6d26111f18 --- /dev/null +++ b/extern/ceres/include/ceres/dynamic_autodiff_cost_function.h @@ -0,0 +1,260 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// mierle@gmail.com (Keir Mierle) +// +// This autodiff implementation differs from the one found in +// autodiff_cost_function.h by supporting autodiff on cost functions +// with variable numbers of parameters with variable sizes. With the +// other implementation, all the sizes (both the number of parameter +// blocks and the size of each block) must be fixed at compile time. +// +// The functor API differs slightly from the API for fixed size +// autodiff; the expected interface for the cost functors is: +// +// struct MyCostFunctor { +// template<typename T> +// bool operator()(T const* const* parameters, T* residuals) const { +// // Use parameters[i] to access the i'th parameter block. +// } +// } +// +// Since the sizing of the parameters is done at runtime, you must +// also specify the sizes after creating the dynamic autodiff cost +// function. For example: +// +// DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function( +// new MyCostFunctor()); +// cost_function.AddParameterBlock(5); +// cost_function.AddParameterBlock(10); +// cost_function.SetNumResiduals(21); +// +// Under the hood, the implementation evaluates the cost function +// multiple times, computing a small set of the derivatives (four by +// default, controlled by the Stride template parameter) with each +// pass. There is a tradeoff with the size of the passes; you may want +// to experiment with the stride. + +#ifndef CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ + +#include <cmath> +#include <numeric> +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/jet.h" +#include "glog/logging.h" + +namespace ceres { + +template <typename CostFunctor, int Stride = 4> +class DynamicAutoDiffCostFunction : public CostFunction { + public: + explicit DynamicAutoDiffCostFunction(CostFunctor* functor) + : functor_(functor) {} + + virtual ~DynamicAutoDiffCostFunction() {} + + void AddParameterBlock(int size) { + mutable_parameter_block_sizes()->push_back(size); + } + + void SetNumResiduals(int num_residuals) { + set_num_residuals(num_residuals); + } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + CHECK_GT(num_residuals(), 0) + << "You must call DynamicAutoDiffCostFunction::SetNumResiduals() " + << "before DynamicAutoDiffCostFunction::Evaluate()."; + + if (jacobians == NULL) { + return (*functor_)(parameters, residuals); + } + + // The difficulty with Jets, as implemented in Ceres, is that they were + // originally designed for strictly compile-sized use. At this point, there + // is a large body of code that assumes inside a cost functor it is + // acceptable to do e.g. T(1.5) and get an appropriately sized jet back. + // + // Unfortunately, it is impossible to communicate the expected size of a + // dynamically sized jet to the static instantiations that existing code + // depends on. + // + // To work around this issue, the solution here is to evaluate the + // jacobians in a series of passes, each one computing Stripe * + // num_residuals() derivatives. This is done with small, fixed-size jets. + const int num_parameter_blocks = parameter_block_sizes().size(); + const int num_parameters = std::accumulate(parameter_block_sizes().begin(), + parameter_block_sizes().end(), + 0); + + // Allocate scratch space for the strided evaluation. + std::vector<Jet<double, Stride> > input_jets(num_parameters); + std::vector<Jet<double, Stride> > output_jets(num_residuals()); + + // Make the parameter pack that is sent to the functor (reused). + std::vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks, + static_cast<Jet<double, Stride>* >(NULL)); + int num_active_parameters = 0; + + // 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. + std::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) { + 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++) { + input_jets[parameter_cursor].a = parameters[i][j]; + } + } + + // 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; + 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 (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[active_parameter_count] = 1.0; + ++active_parameter_count; + ++current_derivative_section_cursor; + } else { + ++current_derivative_section; + current_derivative_section_cursor = 0; + } + } + } + } + + if (!(*functor_)(&jet_parameters[0], &output_jets[0])) { + return false; + } + + // 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 (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[active_parameter_count]; + } + ++active_parameter_count; + ++current_derivative_section_cursor; + } else { + ++current_derivative_section; + current_derivative_section_cursor = 0; + } + } + } + } + + // Only copy the residuals over once (even though we compute them on + // every loop). + if (pass == num_strides - 1) { + for (int k = 0; k < num_residuals(); ++k) { + residuals[k] = output_jets[k].a; + } + } + } + return true; + } + + private: + internal::scoped_ptr<CostFunctor> functor_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/dynamic_cost_function_to_functor.h b/extern/ceres/include/ceres/dynamic_cost_function_to_functor.h new file mode 100644 index 00000000000..9339a503ea0 --- /dev/null +++ b/extern/ceres/include/ceres/dynamic_cost_function_to_functor.h @@ -0,0 +1,190 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// dgossow@google.com (David Gossow) +// +// DynamicCostFunctionToFunctor allows users to use CostFunction +// objects in templated functors which are to be used for automatic +// differentiation. It works similar to CostFunctionToFunctor, with the +// difference that it allows you to wrap a cost function with dynamic numbers +// of parameters and residuals. +// +// For example, let us assume that +// +// class IntrinsicProjection : public CostFunction { +// public: +// IntrinsicProjection(const double* observation); +// virtual bool Evaluate(double const* const* parameters, +// double* residuals, +// double** jacobians) const; +// }; +// +// is a cost function that implements the projection of a point in its +// local coordinate system onto its image plane and subtracts it from +// the observed point projection. It can compute its residual and +// either via analytic or numerical differentiation can compute its +// jacobians. The intrinsics are passed in as parameters[0] and the point as +// parameters[1]. +// +// Now we would like to compose the action of this CostFunction with +// the action of camera extrinsics, i.e., rotation and +// translation. Say we have a templated function +// +// template<typename T> +// void RotateAndTranslatePoint(double const* const* parameters, +// double* residuals); +// +// Then we can now do the following, +// +// struct CameraProjection { +// CameraProjection(const double* observation) +// : intrinsic_projection_.(new IntrinsicProjection(observation)) { +// } +// template <typename T> +// bool operator()(T const* const* parameters, +// T* residual) const { +// const T* rotation = parameters[0]; +// const T* translation = parameters[1]; +// const T* intrinsics = parameters[2]; +// const T* point = parameters[3]; +// T transformed_point[3]; +// RotateAndTranslatePoint(rotation, translation, point, transformed_point); +// +// // Note that we call intrinsic_projection_, just like it was +// // any other templated functor. +// const T* projection_parameters[2]; +// projection_parameters[0] = intrinsics; +// projection_parameters[1] = transformed_point; +// return intrinsic_projection_(projection_parameters, residual); +// } +// +// private: +// DynamicCostFunctionToFunctor intrinsic_projection_; +// }; + +#ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_ +#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_ + +#include <numeric> +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { + +class DynamicCostFunctionToFunctor { + public: + // Takes ownership of cost_function. + explicit DynamicCostFunctionToFunctor(CostFunction* cost_function) + : cost_function_(cost_function) { + CHECK_NOTNULL(cost_function); + } + + bool operator()(double const* const* parameters, double* residuals) const { + return cost_function_->Evaluate(parameters, residuals, NULL); + } + + template <typename JetT> + bool operator()(JetT const* const* inputs, JetT* output) const { + const std::vector<int32>& parameter_block_sizes = + cost_function_->parameter_block_sizes(); + const int num_parameter_blocks = parameter_block_sizes.size(); + const int num_residuals = cost_function_->num_residuals(); + const int num_parameters = std::accumulate(parameter_block_sizes.begin(), + parameter_block_sizes.end(), 0); + + internal::FixedArray<double> parameters(num_parameters); + internal::FixedArray<double*> parameter_blocks(num_parameter_blocks); + internal::FixedArray<double> jacobians(num_residuals * num_parameters); + internal::FixedArray<double*> jacobian_blocks(num_parameter_blocks); + internal::FixedArray<double> residuals(num_residuals); + + // Build a set of arrays to get the residuals and jacobians from + // the CostFunction wrapped by this functor. + double* parameter_ptr = parameters.get(); + double* jacobian_ptr = jacobians.get(); + for (int i = 0; i < num_parameter_blocks; ++i) { + parameter_blocks[i] = parameter_ptr; + jacobian_blocks[i] = jacobian_ptr; + for (int j = 0; j < parameter_block_sizes[i]; ++j) { + *parameter_ptr++ = inputs[i][j].a; + } + jacobian_ptr += num_residuals * parameter_block_sizes[i]; + } + + if (!cost_function_->Evaluate(parameter_blocks.get(), + residuals.get(), + jacobian_blocks.get())) { + return false; + } + + // Now that we have the incoming Jets, which are carrying the + // partial derivatives of each of the inputs w.r.t to some other + // underlying parameters. The derivative of the outputs of the + // cost function w.r.t to the same underlying parameters can now + // be computed by applying the chain rule. + // + // d output[i] d output[i] d input[j] + // -------------- = sum_j ----------- * ------------ + // d parameter[k] d input[j] d parameter[k] + // + // d input[j] + // -------------- = inputs[j], so + // d parameter[k] + // + // outputJet[i] = sum_k jacobian[i][k] * inputJet[k] + // + // The following loop, iterates over the residuals, computing one + // output jet at a time. + for (int i = 0; i < num_residuals; ++i) { + output[i].a = residuals[i]; + output[i].v.setZero(); + + for (int j = 0; j < num_parameter_blocks; ++j) { + const int32 block_size = parameter_block_sizes[j]; + for (int k = 0; k < parameter_block_sizes[j]; ++k) { + output[i].v += + jacobian_blocks[j][i * block_size + k] * inputs[j][k].v; + } + } + } + + return true; + } + + private: + internal::scoped_ptr<CostFunction> cost_function_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_ diff --git a/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h b/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h new file mode 100644 index 00000000000..c852d57a3fc --- /dev/null +++ b/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h @@ -0,0 +1,205 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// sameeragarwal@google.com (Sameer Agarwal) +// thadh@gmail.com (Thad Hughes) +// tbennun@gmail.com (Tal Ben-Nun) +// +// This numeric diff implementation differs from the one found in +// numeric_diff_cost_function.h by supporting numericdiff on cost +// functions with variable numbers of parameters with variable +// sizes. With the other implementation, all the sizes (both the +// number of parameter blocks and the size of each block) must be +// fixed at compile time. +// +// The functor API differs slightly from the API for fixed size +// numeric diff; the expected interface for the cost functors is: +// +// struct MyCostFunctor { +// bool operator()(double const* const* parameters, double* residuals) const { +// // Use parameters[i] to access the i'th parameter block. +// } +// } +// +// Since the sizing of the parameters is done at runtime, you must +// also specify the sizes after creating the +// DynamicNumericDiffCostFunction. For example: +// +// DynamicAutoDiffCostFunction<MyCostFunctor, CENTRAL> cost_function( +// new MyCostFunctor()); +// cost_function.AddParameterBlock(5); +// cost_function.AddParameterBlock(10); +// cost_function.SetNumResiduals(21); + +#ifndef CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ + +#include <cmath> +#include <numeric> +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/numeric_diff.h" +#include "ceres/numeric_diff_options.h" +#include "glog/logging.h" + +namespace ceres { + +template <typename CostFunctor, NumericDiffMethodType method = CENTRAL> +class DynamicNumericDiffCostFunction : public CostFunction { + public: + explicit DynamicNumericDiffCostFunction( + const CostFunctor* functor, + Ownership ownership = TAKE_OWNERSHIP, + const NumericDiffOptions& options = NumericDiffOptions()) + : functor_(functor), + ownership_(ownership), + options_(options) { + } + + // Deprecated. New users should avoid using this constructor. Instead, use the + // constructor with NumericDiffOptions. + DynamicNumericDiffCostFunction( + const CostFunctor* functor, + Ownership ownership, + double relative_step_size) + : functor_(functor), + ownership_(ownership), + options_() { + LOG(WARNING) << "This constructor is deprecated and will be removed in " + "a future version. Please use the NumericDiffOptions " + "constructor instead."; + + options_.relative_step_size = relative_step_size; + } + + virtual ~DynamicNumericDiffCostFunction() { + if (ownership_ != TAKE_OWNERSHIP) { + functor_.release(); + } + } + + void AddParameterBlock(int size) { + mutable_parameter_block_sizes()->push_back(size); + } + + void SetNumResiduals(int num_residuals) { + set_num_residuals(num_residuals); + } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + using internal::NumericDiff; + CHECK_GT(num_residuals(), 0) + << "You must call DynamicNumericDiffCostFunction::SetNumResiduals() " + << "before DynamicNumericDiffCostFunction::Evaluate()."; + + const std::vector<int32>& block_sizes = parameter_block_sizes(); + CHECK(!block_sizes.empty()) + << "You must call DynamicNumericDiffCostFunction::AddParameterBlock() " + << "before DynamicNumericDiffCostFunction::Evaluate()."; + + const bool status = EvaluateCostFunctor(parameters, residuals); + if (jacobians == NULL || !status) { + return status; + } + + // Create local space for a copy of the parameters which will get mutated. + int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0); + std::vector<double> parameters_copy(parameters_size); + std::vector<double*> parameters_references_copy(block_sizes.size()); + parameters_references_copy[0] = ¶meters_copy[0]; + for (int block = 1; block < block_sizes.size(); ++block) { + parameters_references_copy[block] = parameters_references_copy[block - 1] + + block_sizes[block - 1]; + } + + // Copy the parameters into the local temp space. + for (int block = 0; block < block_sizes.size(); ++block) { + memcpy(parameters_references_copy[block], + parameters[block], + block_sizes[block] * sizeof(*parameters[block])); + } + + for (int block = 0; block < block_sizes.size(); ++block) { + if (jacobians[block] != NULL && + !NumericDiff<CostFunctor, method, DYNAMIC, + DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, + DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, + DYNAMIC, DYNAMIC>::EvaluateJacobianForParameterBlock( + functor_.get(), + residuals, + options_, + this->num_residuals(), + block, + block_sizes[block], + ¶meters_references_copy[0], + jacobians[block])) { + return false; + } + } + return true; + } + + private: + bool EvaluateCostFunctor(double const* const* parameters, + double* residuals) const { + return EvaluateCostFunctorImpl(functor_.get(), + parameters, + residuals, + functor_.get()); + } + + // Helper templates to allow evaluation of a functor or a + // CostFunction. + bool EvaluateCostFunctorImpl(const CostFunctor* functor, + double const* const* parameters, + double* residuals, + const void* /* NOT USED */) const { + return (*functor)(parameters, residuals); + } + + bool EvaluateCostFunctorImpl(const CostFunctor* functor, + double const* const* parameters, + double* residuals, + const CostFunction* /* NOT USED */) const { + return functor->Evaluate(parameters, residuals, NULL); + } + + internal::scoped_ptr<const CostFunctor> functor_; + Ownership ownership_; + NumericDiffOptions options_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/fpclassify.h b/extern/ceres/include/ceres/fpclassify.h new file mode 100644 index 00000000000..bc2dc90026c --- /dev/null +++ b/extern/ceres/include/ceres/fpclassify.h @@ -0,0 +1,70 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Portable floating point classification. The names are picked such that they +// do not collide with macros. For example, "isnan" in C99 is a macro and hence +// does not respect namespaces. +// +// TODO(keir): Finish porting! + +#ifndef CERES_PUBLIC_FPCLASSIFY_H_ +#define CERES_PUBLIC_FPCLASSIFY_H_ + +#if defined(_MSC_VER) +#include <float.h> +#endif + +#include <limits> + +namespace ceres { + +#if defined(_MSC_VER) + +inline bool IsFinite (double x) { return _finite(x) != 0; } +inline bool IsInfinite(double x) { return _finite(x) == 0 && _isnan(x) == 0; } +inline bool IsNaN (double x) { return _isnan(x) != 0; } +inline bool IsNormal (double x) { // NOLINT + const int classification = _fpclass(x); + return (classification == _FPCLASS_NN || classification == _FPCLASS_PN); +} + +# else + +// These definitions are for the normal Unix suspects. +inline bool IsFinite (double x) { return std::isfinite(x); } +inline bool IsInfinite(double x) { return std::isinf(x); } +inline bool IsNaN (double x) { return std::isnan(x); } +inline bool IsNormal (double x) { return std::isnormal(x); } + +#endif + +} // namespace ceres + +#endif // CERES_PUBLIC_FPCLASSIFY_H_ diff --git a/extern/ceres/include/ceres/gradient_checker.h b/extern/ceres/include/ceres/gradient_checker.h new file mode 100644 index 00000000000..28304159b44 --- /dev/null +++ b/extern/ceres/include/ceres/gradient_checker.h @@ -0,0 +1,222 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// Copyright 2007 Google Inc. All Rights Reserved. +// +// Author: wjr@google.com (William Rucklidge) +// +// This file contains a class that exercises a cost function, to make sure +// that it is computing reasonable derivatives. It compares the Jacobians +// computed by the cost function with those obtained by finite +// differences. + +#ifndef CERES_PUBLIC_GRADIENT_CHECKER_H_ +#define CERES_PUBLIC_GRADIENT_CHECKER_H_ + +#include <cstddef> +#include <algorithm> +#include <vector> + +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/numeric_diff_cost_function.h" +#include "glog/logging.h" + +namespace ceres { + +// An object that exercises a cost function, to compare the answers that it +// gives with derivatives estimated using finite differencing. +// +// The only likely usage of this is for testing. +// +// How to use: Fill in an array of pointers to parameter blocks for your +// CostFunction, and then call Probe(). Check that the return value is +// 'true'. See prober_test.cc for an example. +// +// This is templated similarly to NumericDiffCostFunction, as it internally +// uses that. +template <typename CostFunctionToProbe, + int M = 0, int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0> +class GradientChecker { + public: + // Here we stash some results from the probe, for later + // inspection. + struct GradientCheckResults { + // Computed cost. + Vector cost; + + // The sizes of these matrices are dictated by the cost function's + // parameter and residual block sizes. Each vector's length will + // term->parameter_block_sizes().size(), and each matrix is the + // Jacobian of the residual with respect to the corresponding parameter + // block. + + // Derivatives as computed by the cost function. + std::vector<Matrix> term_jacobians; + + // Derivatives as computed by finite differencing. + std::vector<Matrix> finite_difference_jacobians; + + // Infinity-norm of term_jacobians - finite_difference_jacobians. + double error_jacobians; + }; + + // Checks the Jacobian computed by a cost function. + // + // probe_point: The parameter values at which to probe. + // error_tolerance: A threshold for the infinity-norm difference + // between the Jacobians. If the Jacobians differ by more than + // this amount, then the probe fails. + // + // term: The cost function to test. Not retained after this call returns. + // + // results: On return, the two Jacobians (and other information) + // will be stored here. May be NULL. + // + // Returns true if no problems are detected and the difference between the + // Jacobians is less than error_tolerance. + static bool Probe(double const* const* probe_point, + double error_tolerance, + CostFunctionToProbe *term, + GradientCheckResults* results) { + CHECK_NOTNULL(probe_point); + CHECK_NOTNULL(term); + LOG(INFO) << "-------------------- Starting Probe() --------------------"; + + // We need a GradientCheckeresults, whether or not they supplied one. + internal::scoped_ptr<GradientCheckResults> owned_results; + if (results == NULL) { + owned_results.reset(new GradientCheckResults); + results = owned_results.get(); + } + + // Do a consistency check between the term and the template parameters. + CHECK_EQ(M, term->num_residuals()); + const int num_residuals = M; + const std::vector<int32>& block_sizes = term->parameter_block_sizes(); + const int num_blocks = block_sizes.size(); + + CHECK_LE(num_blocks, 5) << "Unable to test functions that take more " + << "than 5 parameter blocks"; + if (N0) { + CHECK_EQ(N0, block_sizes[0]); + CHECK_GE(num_blocks, 1); + } else { + CHECK_LT(num_blocks, 1); + } + if (N1) { + CHECK_EQ(N1, block_sizes[1]); + CHECK_GE(num_blocks, 2); + } else { + CHECK_LT(num_blocks, 2); + } + if (N2) { + CHECK_EQ(N2, block_sizes[2]); + CHECK_GE(num_blocks, 3); + } else { + CHECK_LT(num_blocks, 3); + } + if (N3) { + CHECK_EQ(N3, block_sizes[3]); + CHECK_GE(num_blocks, 4); + } else { + CHECK_LT(num_blocks, 4); + } + if (N4) { + CHECK_EQ(N4, block_sizes[4]); + CHECK_GE(num_blocks, 5); + } else { + CHECK_LT(num_blocks, 5); + } + + results->term_jacobians.clear(); + results->term_jacobians.resize(num_blocks); + results->finite_difference_jacobians.clear(); + results->finite_difference_jacobians.resize(num_blocks); + + internal::FixedArray<double*> term_jacobian_pointers(num_blocks); + internal::FixedArray<double*> + finite_difference_jacobian_pointers(num_blocks); + for (int i = 0; i < num_blocks; i++) { + results->term_jacobians[i].resize(num_residuals, block_sizes[i]); + term_jacobian_pointers[i] = results->term_jacobians[i].data(); + results->finite_difference_jacobians[i].resize( + num_residuals, block_sizes[i]); + finite_difference_jacobian_pointers[i] = + results->finite_difference_jacobians[i].data(); + } + results->cost.resize(num_residuals, 1); + + CHECK(term->Evaluate(probe_point, results->cost.data(), + term_jacobian_pointers.get())); + NumericDiffCostFunction<CostFunctionToProbe, CENTRAL, M, N0, N1, N2, N3, N4> + numeric_term(term, DO_NOT_TAKE_OWNERSHIP); + CHECK(numeric_term.Evaluate(probe_point, results->cost.data(), + finite_difference_jacobian_pointers.get())); + + results->error_jacobians = 0; + for (int i = 0; i < num_blocks; i++) { + Matrix jacobian_difference = results->term_jacobians[i] - + results->finite_difference_jacobians[i]; + results->error_jacobians = + std::max(results->error_jacobians, + jacobian_difference.lpNorm<Eigen::Infinity>()); + } + + LOG(INFO) << "========== term-computed derivatives =========="; + for (int i = 0; i < num_blocks; i++) { + LOG(INFO) << "term_computed block " << i; + LOG(INFO) << "\n" << results->term_jacobians[i]; + } + + LOG(INFO) << "========== finite-difference derivatives =========="; + for (int i = 0; i < num_blocks; i++) { + LOG(INFO) << "finite_difference block " << i; + LOG(INFO) << "\n" << results->finite_difference_jacobians[i]; + } + + LOG(INFO) << "========== difference =========="; + for (int i = 0; i < num_blocks; i++) { + LOG(INFO) << "difference block " << i; + LOG(INFO) << (results->term_jacobians[i] - + results->finite_difference_jacobians[i]); + } + + LOG(INFO) << "||difference|| = " << results->error_jacobians; + + return results->error_jacobians < error_tolerance; + } + + private: + CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(GradientChecker); +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_GRADIENT_CHECKER_H_ diff --git a/extern/ceres/include/ceres/gradient_problem.h b/extern/ceres/include/ceres/gradient_problem.h new file mode 100644 index 00000000000..1226a4cd895 --- /dev/null +++ b/extern/ceres/include/ceres/gradient_problem.h @@ -0,0 +1,127 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_GRADIENT_PROBLEM_H_ +#define CERES_PUBLIC_GRADIENT_PROBLEM_H_ + +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/local_parameterization.h" + +namespace ceres { + +class FirstOrderFunction; + +// Instances of GradientProblem represent general non-linear +// optimization problems that must be solved using just the value of +// the objective function and its gradient. Unlike the Problem class, +// which can only be used to model non-linear least squares problems, +// instances of GradientProblem not restricted in the form of the +// objective function. +// +// Structurally GradientProblem is a composition of a +// FirstOrderFunction and optionally a LocalParameterization. +// +// The FirstOrderFunction is responsible for evaluating the cost and +// gradient of the objective function. +// +// The LocalParameterization is responsible for going back and forth +// between the ambient space and the local tangent space. (See +// local_parameterization.h for more details). When a +// LocalParameterization is not provided, then the tangent space is +// assumed to coincide with the ambient Euclidean space that the +// gradient vector lives in. +// +// Example usage: +// +// The following demonstrate the problem construction for Rosenbrock's function +// +// f(x,y) = (1-x)^2 + 100(y - x^2)^2; +// +// class Rosenbrock : public ceres::FirstOrderFunction { +// public: +// virtual ~Rosenbrock() {} +// +// virtual bool Evaluate(const double* parameters, +// double* cost, +// double* gradient) const { +// const double x = parameters[0]; +// const double y = parameters[1]; +// +// cost[0] = (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x); +// if (gradient != NULL) { +// gradient[0] = -2.0 * (1.0 - x) - 200.0 * (y - x * x) * 2.0 * x; +// gradient[1] = 200.0 * (y - x * x); +// } +// return true; +// }; +// +// virtual int NumParameters() const { return 2; }; +// }; +// +// ceres::GradientProblem problem(new Rosenbrock()); +class CERES_EXPORT GradientProblem { + public: + // Takes ownership of the function. + explicit GradientProblem(FirstOrderFunction* function); + + // Takes ownership of the function and the parameterization. + GradientProblem(FirstOrderFunction* function, + LocalParameterization* parameterization); + + int NumParameters() const; + int NumLocalParameters() const; + + // This call is not thread safe. + bool Evaluate(const double* parameters, double* cost, double* gradient) const; + bool Plus(const double* x, const double* delta, double* x_plus_delta) const; + + private: + internal::scoped_ptr<FirstOrderFunction> function_; + internal::scoped_ptr<LocalParameterization> parameterization_; + internal::scoped_array<double> scratch_; +}; + +// A FirstOrderFunction object implements the evaluation of a function +// and its gradient. +class CERES_EXPORT FirstOrderFunction { + public: + virtual ~FirstOrderFunction() {} + // cost is never NULL. gradient may be null. + virtual bool Evaluate(const double* const parameters, + double* cost, + double* gradient) const = 0; + virtual int NumParameters() const = 0; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_GRADIENT_PROBLEM_H_ diff --git a/extern/ceres/include/ceres/gradient_problem_solver.h b/extern/ceres/include/ceres/gradient_problem_solver.h new file mode 100644 index 00000000000..a7d0121ea0c --- /dev/null +++ b/extern/ceres/include/ceres/gradient_problem_solver.h @@ -0,0 +1,357 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_GRADIENT_PROBLEM_SOLVER_H_ +#define CERES_PUBLIC_GRADIENT_PROBLEM_SOLVER_H_ + +#include <cmath> +#include <string> +#include <vector> +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/iteration_callback.h" +#include "ceres/types.h" +#include "ceres/internal/disable_warnings.h" + +namespace ceres { + +class GradientProblem; + +class CERES_EXPORT GradientProblemSolver { + public: + virtual ~GradientProblemSolver(); + + // The options structure contains, not surprisingly, options that control how + // the solver operates. The defaults should be suitable for a wide range of + // problems; however, better performance is often obtainable with tweaking. + // + // The constants are defined inside types.h + struct CERES_EXPORT Options { + // Default constructor that sets up a generic sparse problem. + Options() { + line_search_direction_type = LBFGS; + 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; + max_num_iterations = 50; + max_solver_time_in_seconds = 1e9; + function_tolerance = 1e-6; + gradient_tolerance = 1e-10; + logging_type = PER_MINIMIZER_ITERATION; + minimizer_progress_to_stdout = false; + } + + // Returns true if the options struct has a valid + // configuration. Returns false otherwise, and fills in *error + // with a message describing the problem. + bool IsValid(std::string* error) const; + + // Minimizer options ---------------------------------------- + LineSearchDirectionType line_search_direction_type; + LineSearchType line_search_type; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; + + // The LBFGS hessian approximation is a low rank approximation to + // the inverse of the Hessian matrix. The rank of the + // approximation determines (linearly) the space and time + // complexity of using the approximation. Higher the rank, the + // better is the quality of the approximation. The increase in + // quality is however is bounded for a number of reasons. + // + // 1. The method only uses secant information and not actual + // derivatives. + // + // 2. The Hessian approximation is constrained to be positive + // definite. + // + // So increasing this rank to a large number will cost time and + // space complexity without the corresponding increase in solution + // quality. There are no hard and fast rules for choosing the + // maximum rank. The best choice usually requires some problem + // specific experimentation. + // + // For more theoretical and implementation details of the LBFGS + // method, please see: + // + // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with + // 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; + + // Maximum number of iterations for the minimizer to run for. + int max_num_iterations; + + // Maximum time for which the minimizer should run for. + double max_solver_time_in_seconds; + + // Minimizer terminates when + // + // (new_cost - old_cost) < function_tolerance * old_cost; + // + double function_tolerance; + + // Minimizer terminates when + // + // max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance + // + // This value should typically be 1e-4 * function_tolerance. + double gradient_tolerance; + + // Logging options --------------------------------------------------------- + + LoggingType logging_type; + + // By default the Minimizer progress is logged to VLOG(1), which + // is sent to STDERR depending on the vlog level. If this flag is + // set to true, and logging_type is not SILENT, the logging output + // is sent to STDOUT. + bool minimizer_progress_to_stdout; + + // Callbacks that are executed at the end of each iteration of the + // Minimizer. An iteration may terminate midway, either due to + // numerical failures or because one of the convergence tests has + // been satisfied. In this case none of the callbacks are + // executed. + + // Callbacks are executed in the order that they are specified in + // this vector. By default, parameter blocks are updated only at + // the end of the optimization, i.e when the Minimizer + // terminates. This behaviour is controlled by + // update_state_every_variable. If the user wishes to have access + // to the update parameter blocks when his/her callbacks are + // executed, then set update_state_every_iteration to true. + // + // The solver does NOT take ownership of these pointers. + std::vector<IterationCallback*> callbacks; + }; + + struct CERES_EXPORT Summary { + Summary(); + + // A brief one line description of the state of the solver after + // termination. + std::string BriefReport() const; + + // A full multiline description of the state of the solver after + // termination. + std::string FullReport() const; + + bool IsSolutionUsable() const; + + // Minimizer summary ------------------------------------------------- + TerminationType termination_type; + + // Reason why the solver terminated. + std::string message; + + // Cost of the problem (value of the objective function) before + // the optimization. + double initial_cost; + + // Cost of the problem (value of the objective function) after the + // optimization. + double final_cost; + + // IterationSummary for each minimizer iteration in order. + std::vector<IterationSummary> iterations; + + // Sum total of all time spent inside Ceres when Solve is called. + double total_time_in_seconds; + + // Time (in seconds) spent evaluating the cost. + double cost_evaluation_time_in_seconds; + + // Time (in seconds) spent evaluating the gradient. + double gradient_evaluation_time_in_seconds; + + // Time (in seconds) spent minimizing the interpolating polynomial + // to compute the next candidate step size as part of a line search. + double line_search_polynomial_minimization_time_in_seconds; + + // Number of parameters in the probem. + int num_parameters; + + // Dimension of the tangent space of the problem. + int num_local_parameters; + + // Type of line search direction used. + LineSearchDirectionType line_search_direction_type; + + // Type of the line search algorithm used. + LineSearchType line_search_type; + + // When performing line search, the degree of the polynomial used + // to approximate the objective function. + LineSearchInterpolationType line_search_interpolation_type; + + // If the line search direction is NONLINEAR_CONJUGATE_GRADIENT, + // then this indicates the particular variant of non-linear + // conjugate gradient used. + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; + + // If the type of the line search direction is LBFGS, then this + // indicates the rank of the Hessian approximation. + int max_lbfgs_rank; + }; + + // Once a least squares problem has been built, this function takes + // the problem and optimizes it based on the values of the options + // parameters. Upon return, a detailed summary of the work performed + // by the preprocessor, the non-linear minmizer and the linear + // solver are reported in the summary object. + virtual void Solve(const GradientProblemSolver::Options& options, + const GradientProblem& problem, + double* parameters, + GradientProblemSolver::Summary* summary); +}; + +// Helper function which avoids going through the interface. +CERES_EXPORT void Solve(const GradientProblemSolver::Options& options, + const GradientProblem& problem, + double* parameters, + GradientProblemSolver::Summary* summary); + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_GRADIENT_PROBLEM_SOLVER_H_ diff --git a/extern/ceres/include/ceres/internal/autodiff.h b/extern/ceres/include/ceres/internal/autodiff.h new file mode 100644 index 00000000000..136152a36cd --- /dev/null +++ b/extern/ceres/include/ceres/internal/autodiff.h @@ -0,0 +1,317 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Computation of the Jacobian matrix for vector-valued functions of multiple +// variables, using automatic differentiation based on the implementation of +// dual numbers in jet.h. Before reading the rest of this file, it is adivsable +// to read jet.h's header comment in detail. +// +// The helper wrapper AutoDiff::Differentiate() computes the jacobian of +// functors with templated operator() taking this form: +// +// struct F { +// template<typename T> +// bool operator()(const T *x, const T *y, ..., T *z) { +// // Compute z[] based on x[], y[], ... +// // return true if computation succeeded, false otherwise. +// } +// }; +// +// All inputs and outputs may be vector-valued. +// +// To understand how jets are used to compute the jacobian, a +// picture may help. Consider a vector-valued function, F, returning 3 +// dimensions and taking a vector-valued parameter of 4 dimensions: +// +// y x +// [ * ] F [ * ] +// [ * ] <--- [ * ] +// [ * ] [ * ] +// [ * ] +// +// Similar to the 2-parameter example for f described in jet.h, computing the +// jacobian dy/dx is done by substutiting a suitable jet object for x and all +// intermediate steps of the computation of F. Since x is has 4 dimensions, use +// a Jet<double, 4>. +// +// Before substituting a jet object for x, the dual components are set +// appropriately for each dimension of x: +// +// y x +// [ * | * * * * ] f [ * | 1 0 0 0 ] x0 +// [ * | * * * * ] <--- [ * | 0 1 0 0 ] x1 +// [ * | * * * * ] [ * | 0 0 1 0 ] x2 +// ---+--- [ * | 0 0 0 1 ] x3 +// | ^ ^ ^ ^ +// dy/dx | | | +----- infinitesimal for x3 +// | | +------- infinitesimal for x2 +// | +--------- infinitesimal for x1 +// +----------- infinitesimal for x0 +// +// The reason to set the internal 4x4 submatrix to the identity is that we wish +// to take the derivative of y separately with respect to each dimension of x. +// Each column of the 4x4 identity is therefore for a single component of the +// independent variable x. +// +// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the +// extended y vector, indicated in the above diagram. +// +// Functors with multiple parameters +// --------------------------------- +// In practice, it is often convenient to use a function f of two or more +// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet +// framework is designed for a single-parameter vector-valued input. The wrapper +// in this file addresses this issue adding support for functions with one or +// more parameter vectors. +// +// To support multiple parameters, all the parameter vectors are concatenated +// into one and treated as a single parameter vector, except that since the +// functor expects different inputs, we need to construct the jets as if they +// were part of a single parameter vector. The extended jets are passed +// separately for each parameter. +// +// For example, consider a functor F taking two vector parameters, p[2] and +// q[3], and producing an output y[4]: +// +// struct F { +// template<typename T> +// bool operator()(const T *p, const T *q, T *z) { +// // ... +// } +// }; +// +// In this case, the necessary jet type is Jet<double, 5>. Here is a +// visualization of the jet objects in this case: +// +// Dual components for p ----+ +// | +// -+- +// y [ * | 1 0 | 0 0 0 ] --- p[0] +// [ * | 0 1 | 0 0 0 ] --- p[1] +// [ * | . . | + + + ] | +// [ * | . . | + + + ] v +// [ * | . . | + + + ] <--- F(p, q) +// [ * | . . | + + + ] ^ +// ^^^ ^^^^^ | +// dy/dp dy/dq [ * | 0 0 | 1 0 0 ] --- q[0] +// [ * | 0 0 | 0 1 0 ] --- q[1] +// [ * | 0 0 | 0 0 1 ] --- q[2] +// --+-- +// | +// Dual components for q --------------+ +// +// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+" +// of y in the above diagram are the derivatives of y with respect to p and q +// respectively. This is how autodiff works for functors taking multiple vector +// valued arguments (up to 6). +// +// Jacobian NULL pointers +// ---------------------- +// In general, the functions below will accept NULL pointers for all or some of +// the Jacobian parameters, meaning that those Jacobians will not be computed. + +#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_ +#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_ + +#include <stddef.h> + +#include "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 { + +// Extends src by a 1st order pertubation for every dimension and puts it in +// dst. The size of src is N. Since this is also used for perturbations in +// blocked arrays, offset is used to shift which part of the jet the +// perturbation occurs. This is used to set up the extended x augmented by an +// identity matrix. The JetT type should be a Jet type, and T should be a +// numeric type (e.g. double). For example, +// +// 0 1 2 3 4 5 6 7 8 +// dst[0] [ * | . . | 1 0 0 | . . . ] +// dst[1] [ * | . . | 0 1 0 | . . . ] +// dst[2] [ * | . . | 0 0 1 | . . . ] +// +// is what would get put in dst if N was 3, offset was 3, and the jet type JetT +// was 8-dimensional. +template <typename JetT, typename T, 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].a = src[j]; + dst[j].v.setZero(); + dst[j].v[offset + j] = T(1.0); + } +} + +// Takes the 0th order part of src, assumed to be a Jet type, and puts it in +// dst. This is used to pick out the "vector" part of the extended y. +template <typename JetT, typename T> +inline void Take0thOrderPart(int M, const JetT *src, T dst) { + DCHECK(src); + for (int i = 0; i < M; ++i) { + dst[i] = src[i].a; + } +} + +// Takes N 1st order parts, starting at index N0, and puts them in the M x N +// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y. +template <typename JetT, typename T, int N0, int N> +inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { + DCHECK(src); + DCHECK(dst); + for (int i = 0; i < M; ++i) { + Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) = + src[i].v.template segment<N>(N0); + } +} + +// This is in a struct because default template parameters on a +// function are not supported in C++03 (though it is available in +// C++0x). N0 through N5 are the dimension of the input arguments to +// the user supplied functor. +template <typename Functor, typename T, + int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, + int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> +struct AutoDiff { + static bool Differentiate(const Functor& functor, + T const *const *parameters, + int num_outputs, + T *function_value, + T **jacobians) { + // This block breaks the 80 column rule to keep it somewhat readable. + DCHECK_GT(num_outputs, 0); + 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) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT + << "Zero block cannot precede a non-zero block. Block sizes are " + << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " + << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " + << N8 << ", " << N9; + + typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT; + FixedArray<JetT, (256 * 7) / sizeof(JetT)> x( + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs); + + // These are the positions of the respective jets in the fixed array x. + const int jet0 = 0; + const int jet1 = N0; + const int jet2 = N0 + N1; + const int jet3 = N0 + N1 + N2; + const int jet4 = N0 + N1 + N2 + N3; + const int jet5 = N0 + N1 + N2 + N3 + N4; + const int jet6 = N0 + N1 + N2 + N3 + N4 + N5; + const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6; + const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7; + const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8; + + const JetT *unpacked_parameters[10] = { + x.get() + jet0, + x.get() + jet1, + x.get() + jet2, + x.get() + jet3, + x.get() + jet4, + x.get() + jet5, + x.get() + jet6, + x.get() + jet7, + x.get() + jet8, + x.get() + jet9, + }; + + 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<JetT, T, N ## i>( \ + jet ## i, \ + parameters[i], \ + x.get() + jet ## i); \ + } + CERES_MAKE_1ST_ORDER_PERTURBATION(0); + CERES_MAKE_1ST_ORDER_PERTURBATION(1); + CERES_MAKE_1ST_ORDER_PERTURBATION(2); + CERES_MAKE_1ST_ORDER_PERTURBATION(3); + CERES_MAKE_1ST_ORDER_PERTURBATION(4); + CERES_MAKE_1ST_ORDER_PERTURBATION(5); + CERES_MAKE_1ST_ORDER_PERTURBATION(6); + CERES_MAKE_1ST_ORDER_PERTURBATION(7); + CERES_MAKE_1ST_ORDER_PERTURBATION(8); + CERES_MAKE_1ST_ORDER_PERTURBATION(9); +#undef CERES_MAKE_1ST_ORDER_PERTURBATION + + if (!VariadicEvaluate<Functor, JetT, + N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call( + functor, unpacked_parameters, output)) { + return false; + } + + internal::Take0thOrderPart(num_outputs, output, function_value); + +#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \ + if (N ## i) { \ + if (jacobians[i]) { \ + internal::Take1stOrderPart<JetT, T, \ + jet ## i, \ + N ## i>(num_outputs, \ + output, \ + jacobians[i]); \ + } \ + } + CERES_TAKE_1ST_ORDER_PERTURBATION(0); + CERES_TAKE_1ST_ORDER_PERTURBATION(1); + CERES_TAKE_1ST_ORDER_PERTURBATION(2); + CERES_TAKE_1ST_ORDER_PERTURBATION(3); + CERES_TAKE_1ST_ORDER_PERTURBATION(4); + CERES_TAKE_1ST_ORDER_PERTURBATION(5); + CERES_TAKE_1ST_ORDER_PERTURBATION(6); + CERES_TAKE_1ST_ORDER_PERTURBATION(7); + CERES_TAKE_1ST_ORDER_PERTURBATION(8); + CERES_TAKE_1ST_ORDER_PERTURBATION(9); +#undef CERES_TAKE_1ST_ORDER_PERTURBATION + return true; + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_ diff --git a/extern/ceres/include/ceres/internal/disable_warnings.h b/extern/ceres/include/ceres/internal/disable_warnings.h new file mode 100644 index 00000000000..094124f7159 --- /dev/null +++ b/extern/ceres/include/ceres/internal/disable_warnings.h @@ -0,0 +1,44 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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. +// +// This file has the sole purpose to silence warnings when including Ceres. + +// This is not your usual header guard. The macro CERES_WARNINGS_DISABLED +// shows up again in reenable_warnings.h. +#ifndef CERES_WARNINGS_DISABLED +#define CERES_WARNINGS_DISABLED + +#ifdef _MSC_VER +#pragma warning( push ) +// Disable the warning C4251 which is trigerred by stl classes in +// Ceres' public interface. To quote MSDN: "C4251 can be ignored " +// "if you are deriving from a type in the Standard C++ Library" +#pragma warning( disable : 4251 ) +#endif + +#endif // CERES_WARNINGS_DISABLED diff --git a/extern/ceres/include/ceres/internal/eigen.h b/extern/ceres/include/ceres/internal/eigen.h new file mode 100644 index 00000000000..7138804ace4 --- /dev/null +++ b/extern/ceres/include/ceres/internal/eigen.h @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_EIGEN_H_ +#define CERES_INTERNAL_EIGEN_H_ + +#include "Eigen/Core" + +namespace ceres { + +typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector; +typedef Eigen::Matrix<double, + Eigen::Dynamic, + Eigen::Dynamic, + Eigen::RowMajor> Matrix; +typedef Eigen::Map<Vector> VectorRef; +typedef Eigen::Map<Matrix> MatrixRef; +typedef Eigen::Map<const Vector> ConstVectorRef; +typedef Eigen::Map<const Matrix> ConstMatrixRef; + +// Column major matrices for DenseSparseMatrix/DenseQRSolver +typedef Eigen::Matrix<double, + Eigen::Dynamic, + Eigen::Dynamic, + Eigen::ColMajor> ColMajorMatrix; + +typedef Eigen::Map<ColMajorMatrix, 0, + Eigen::Stride<Eigen::Dynamic, 1> > ColMajorMatrixRef; + +typedef Eigen::Map<const ColMajorMatrix, + 0, + Eigen::Stride<Eigen::Dynamic, 1> > ConstColMajorMatrixRef; + + + +// C++ does not support templated typdefs, thus the need for this +// struct so that we can support statically sized Matrix and Maps. +template <int num_rows = Eigen::Dynamic, int num_cols = Eigen::Dynamic> +struct EigenTypes { + typedef Eigen::Matrix <double, num_rows, num_cols, Eigen::RowMajor> + Matrix; + + typedef Eigen::Map< + Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> > + MatrixRef; + + typedef Eigen::Matrix <double, num_rows, 1> + Vector; + + typedef Eigen::Map < + Eigen::Matrix<double, num_rows, 1> > + VectorRef; + + + typedef Eigen::Map< + const Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> > + ConstMatrixRef; + + typedef Eigen::Map < + const Eigen::Matrix<double, num_rows, 1> > + ConstVectorRef; +}; + +} // namespace ceres + +#endif // CERES_INTERNAL_EIGEN_H_ diff --git a/extern/ceres/include/ceres/internal/fixed_array.h b/extern/ceres/include/ceres/internal/fixed_array.h new file mode 100644 index 00000000000..387298c58d0 --- /dev/null +++ b/extern/ceres/include/ceres/internal/fixed_array.h @@ -0,0 +1,191 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: rennie@google.com (Jeffrey Rennie) +// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray + +#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ +#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ + +#include <cstddef> +#include "Eigen/Core" +#include "ceres/internal/macros.h" +#include "ceres/internal/manual_constructor.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// A FixedArray<T> represents a non-resizable array of T where the +// length of the array does not need to be a compile time constant. +// +// FixedArray allocates small arrays inline, and large arrays on +// the heap. It is a good replacement for non-standard and deprecated +// uses of alloca() and variable length arrays (a GCC extension). +// +// FixedArray keeps performance fast for small arrays, because it +// avoids heap operations. It also helps reduce the chances of +// accidentally overflowing your stack if large input is passed to +// your function. +// +// Also, FixedArray is useful for writing portable code. Not all +// compilers support arrays of dynamic size. + +// Most users should not specify an inline_elements argument and let +// FixedArray<> automatically determine the number of elements +// to store inline based on sizeof(T). +// +// If inline_elements is specified, the FixedArray<> implementation +// will store arrays of length <= inline_elements inline. +// +// Finally note that unlike vector<T> FixedArray<T> will not zero-initialize +// simple types like int, double, bool, etc. +// +// Non-POD types will be default-initialized just like regular vectors or +// arrays. + +#if defined(_WIN64) + typedef __int64 ssize_t; +#elif defined(_WIN32) + typedef __int32 ssize_t; +#endif + +template <typename T, ssize_t inline_elements = -1> +class FixedArray { + public: + // For playing nicely with stl: + typedef T value_type; + typedef T* iterator; + typedef T const* const_iterator; + typedef T& reference; + typedef T const& const_reference; + typedef T* pointer; + typedef std::ptrdiff_t difference_type; + typedef size_t size_type; + + // REQUIRES: n >= 0 + // Creates an array object that can store "n" elements. + // + // FixedArray<T> will not zero-initialiaze POD (simple) types like int, + // double, bool, etc. + // Non-POD types will be default-initialized just like regular vectors or + // arrays. + explicit FixedArray(size_type n); + + // Releases any resources. + ~FixedArray(); + + // Returns the length of the array. + inline size_type size() const { return size_; } + + // Returns the memory size of the array in bytes. + inline size_t memsize() const { return size_ * sizeof(T); } + + // Returns a pointer to the underlying element array. + inline const T* get() const { return &array_[0].element; } + inline T* get() { return &array_[0].element; } + + // REQUIRES: 0 <= i < size() + // Returns a reference to the "i"th element. + inline T& operator[](size_type i) { + DCHECK_LT(i, size_); + return array_[i].element; + } + + // REQUIRES: 0 <= i < size() + // Returns a reference to the "i"th element. + inline const T& operator[](size_type i) const { + DCHECK_LT(i, size_); + return array_[i].element; + } + + inline iterator begin() { return &array_[0].element; } + inline iterator end() { return &array_[size_].element; } + + inline const_iterator begin() const { return &array_[0].element; } + inline const_iterator end() const { return &array_[size_].element; } + + private: + // Container to hold elements of type T. This is necessary to handle + // the case where T is a a (C-style) array. The size of InnerContainer + // and T must be the same, otherwise callers' assumptions about use + // of this code will be broken. + struct InnerContainer { + T element; + }; + + // How many elements should we store inline? + // a. If not specified, use a default of 256 bytes (256 bytes + // seems small enough to not cause stack overflow or unnecessary + // stack pollution, while still allowing stack allocation for + // reasonably long character arrays. + // b. Never use 0 length arrays (not ISO C++) + static const size_type S1 = ((inline_elements < 0) + ? (256/sizeof(T)) : inline_elements); + static const size_type S2 = (S1 <= 0) ? 1 : S1; + static const size_type kInlineElements = S2; + + size_type const size_; + InnerContainer* const array_; + + // Allocate some space, not an array of elements of type T, so that we can + // skip calling the T constructors and destructors for space we never use. + ManualConstructor<InnerContainer> inline_space_[kInlineElements]; +}; + +// Implementation details follow + +template <class T, ssize_t S> +inline FixedArray<T, S>::FixedArray(typename FixedArray<T, S>::size_type n) + : size_(n), + array_((n <= kInlineElements + ? reinterpret_cast<InnerContainer*>(inline_space_) + : new InnerContainer[n])) { + // Construct only the elements actually used. + if (array_ == reinterpret_cast<InnerContainer*>(inline_space_)) { + for (size_t i = 0; i != size_; ++i) { + inline_space_[i].Init(); + } + } +} + +template <class T, ssize_t S> +inline FixedArray<T, S>::~FixedArray() { + if (array_ != reinterpret_cast<InnerContainer*>(inline_space_)) { + delete[] array_; + } else { + for (size_t i = 0; i != size_; ++i) { + inline_space_[i].Destroy(); + } + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ diff --git a/extern/ceres/include/ceres/internal/macros.h b/extern/ceres/include/ceres/internal/macros.h new file mode 100644 index 00000000000..bebb965e25b --- /dev/null +++ b/extern/ceres/include/ceres/internal/macros.h @@ -0,0 +1,170 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// +// Various Google-specific macros. +// +// This code is compiled directly on many platforms, including client +// platforms like Windows, Mac, and embedded systems. Before making +// any changes here, make sure that you're not breaking any platforms. + +#ifndef CERES_PUBLIC_INTERNAL_MACROS_H_ +#define CERES_PUBLIC_INTERNAL_MACROS_H_ + +#include <cstddef> // For size_t. + +// A macro to disallow the copy constructor and operator= functions +// This should be used in the private: declarations for a class +// +// For disallowing only assign or copy, write the code directly, but declare +// the intend in a comment, for example: +// +// void operator=(const TypeName&); // _DISALLOW_ASSIGN + +// Note, that most uses of CERES_DISALLOW_ASSIGN and CERES_DISALLOW_COPY +// are broken semantically, one should either use disallow both or +// neither. Try to avoid these in new code. +#define CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) \ + TypeName(const TypeName&); \ + void operator=(const TypeName&) + +// A macro to disallow all the implicit constructors, namely the +// default constructor, copy constructor and operator= functions. +// +// This should be used in the private: declarations for a class +// that wants to prevent anyone from instantiating it. This is +// especially useful for classes containing only static methods. +#define CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(TypeName) \ + TypeName(); \ + CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) + +// The arraysize(arr) macro returns the # of elements in an array arr. +// The expression is a compile-time constant, and therefore can be +// used in defining new arrays, for example. If you use arraysize on +// a pointer by mistake, you will get a compile-time error. +// +// One caveat is that arraysize() doesn't accept any array of an +// anonymous type or a type defined inside a function. In these rare +// cases, you have to use the unsafe ARRAYSIZE() macro below. This is +// due to a limitation in C++'s template system. The limitation might +// eventually be removed, but it hasn't happened yet. + +// This template function declaration is used in defining arraysize. +// Note that the function doesn't need an implementation, as we only +// use its type. +template <typename T, size_t N> +char (&ArraySizeHelper(T (&array)[N]))[N]; + +// That gcc wants both of these prototypes seems mysterious. VC, for +// its part, can't decide which to use (another mystery). Matching of +// template overloads: the final frontier. +#ifndef _WIN32 +template <typename T, size_t N> +char (&ArraySizeHelper(const T (&array)[N]))[N]; +#endif + +#define arraysize(array) (sizeof(ArraySizeHelper(array))) + +// ARRAYSIZE performs essentially the same calculation as arraysize, +// but can be used on anonymous types or types defined inside +// functions. It's less safe than arraysize as it accepts some +// (although not all) pointers. Therefore, you should use arraysize +// whenever possible. +// +// The expression ARRAYSIZE(a) is a compile-time constant of type +// size_t. +// +// ARRAYSIZE catches a few type errors. If you see a compiler error +// +// "warning: division by zero in ..." +// +// when using ARRAYSIZE, you are (wrongfully) giving it a pointer. +// You should only use ARRAYSIZE on statically allocated arrays. +// +// The following comments are on the implementation details, and can +// be ignored by the users. +// +// ARRAYSIZE(arr) works by inspecting sizeof(arr) (the # of bytes in +// the array) and sizeof(*(arr)) (the # of bytes in one array +// element). If the former is divisible by the latter, perhaps arr is +// indeed an array, in which case the division result is the # of +// elements in the array. Otherwise, arr cannot possibly be an array, +// and we generate a compiler error to prevent the code from +// compiling. +// +// Since the size of bool is implementation-defined, we need to cast +// !(sizeof(a) & sizeof(*(a))) to size_t in order to ensure the final +// result has type size_t. +// +// This macro is not perfect as it wrongfully accepts certain +// pointers, namely where the pointer size is divisible by the pointee +// size. Since all our code has to go through a 32-bit compiler, +// where a pointer is 4 bytes, this means all pointers to a type whose +// size is 3 or greater than 4 will be (righteously) rejected. +// +// Kudos to Jorg Brown for this simple and elegant implementation. +// +// - wan 2005-11-16 +// +// Starting with Visual C++ 2005, WinNT.h includes ARRAYSIZE. However, +// the definition comes from the over-broad windows.h header that +// introduces a macro, ERROR, that conflicts with the logging framework +// that Ceres uses. Instead, rename ARRAYSIZE to CERES_ARRAYSIZE. +#define CERES_ARRAYSIZE(a) \ + ((sizeof(a) / sizeof(*(a))) / \ + static_cast<size_t>(!(sizeof(a) % sizeof(*(a))))) + +// Tell the compiler to warn about unused return values for functions +// declared with this macro. The macro should be used on function +// declarations following the argument list: +// +// Sprocket* AllocateSprocket() MUST_USE_RESULT; +// +#if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) \ + && !defined(COMPILER_ICC) +#define CERES_MUST_USE_RESULT __attribute__ ((warn_unused_result)) +#else +#define CERES_MUST_USE_RESULT +#endif + +// Platform independent macros to get aligned memory allocations. +// For example +// +// MyFoo my_foo CERES_ALIGN_ATTRIBUTE(16); +// +// Gives us an instance of MyFoo which is aligned at a 16 byte +// boundary. +#if defined(_MSC_VER) +#define CERES_ALIGN_ATTRIBUTE(n) __declspec(align(n)) +#define CERES_ALIGN_OF(T) __alignof(T) +#elif defined(__GNUC__) +#define CERES_ALIGN_ATTRIBUTE(n) __attribute__((aligned(n))) +#define CERES_ALIGN_OF(T) __alignof(T) +#endif + +#endif // CERES_PUBLIC_INTERNAL_MACROS_H_ diff --git a/extern/ceres/include/ceres/internal/manual_constructor.h b/extern/ceres/include/ceres/internal/manual_constructor.h new file mode 100644 index 00000000000..0d7633cef83 --- /dev/null +++ b/extern/ceres/include/ceres/internal/manual_constructor.h @@ -0,0 +1,208 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: kenton@google.com (Kenton Varda) +// +// ManualConstructor statically-allocates space in which to store some +// object, but does not initialize it. You can then call the constructor +// and destructor for the object yourself as you see fit. This is useful +// for memory management optimizations, where you want to initialize and +// destroy an object multiple times but only allocate it once. +// +// (When I say ManualConstructor statically allocates space, I mean that +// the ManualConstructor object itself is forced to be the right size.) + +#ifndef CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ +#define CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ + +#include <new> + +namespace ceres { +namespace internal { + +// ------- Define CERES_ALIGNED_CHAR_ARRAY -------------------------------- + +#ifndef CERES_ALIGNED_CHAR_ARRAY + +// Because MSVC and older GCCs require that the argument to their alignment +// construct to be a literal constant integer, we use a template instantiated +// at all the possible powers of two. +template<int alignment, int size> struct AlignType { }; +template<int size> struct AlignType<0, size> { typedef char result[size]; }; + +#if !defined(CERES_ALIGN_ATTRIBUTE) +#define CERES_ALIGNED_CHAR_ARRAY you_must_define_CERES_ALIGNED_CHAR_ARRAY_for_your_compiler +#else // !defined(CERES_ALIGN_ATTRIBUTE) + +#define CERES_ALIGN_TYPE_TEMPLATE(X) \ + template<int size> struct AlignType<X, size> { \ + typedef CERES_ALIGN_ATTRIBUTE(X) char result[size]; \ + } + +CERES_ALIGN_TYPE_TEMPLATE(1); +CERES_ALIGN_TYPE_TEMPLATE(2); +CERES_ALIGN_TYPE_TEMPLATE(4); +CERES_ALIGN_TYPE_TEMPLATE(8); +CERES_ALIGN_TYPE_TEMPLATE(16); +CERES_ALIGN_TYPE_TEMPLATE(32); +CERES_ALIGN_TYPE_TEMPLATE(64); +CERES_ALIGN_TYPE_TEMPLATE(128); +CERES_ALIGN_TYPE_TEMPLATE(256); +CERES_ALIGN_TYPE_TEMPLATE(512); +CERES_ALIGN_TYPE_TEMPLATE(1024); +CERES_ALIGN_TYPE_TEMPLATE(2048); +CERES_ALIGN_TYPE_TEMPLATE(4096); +CERES_ALIGN_TYPE_TEMPLATE(8192); +// Any larger and MSVC++ will complain. + +#undef CERES_ALIGN_TYPE_TEMPLATE + +#define CERES_ALIGNED_CHAR_ARRAY(T, Size) \ + typename AlignType<CERES_ALIGN_OF(T), sizeof(T) * Size>::result + +#endif // !defined(CERES_ALIGN_ATTRIBUTE) + +#endif // CERES_ALIGNED_CHAR_ARRAY + +template <typename Type> +class ManualConstructor { + public: + // No constructor or destructor because one of the most useful uses of + // this class is as part of a union, and members of a union cannot have + // constructors or destructors. And, anyway, the whole point of this + // class is to bypass these. + + inline Type* get() { + return reinterpret_cast<Type*>(space_); + } + inline const Type* get() const { + return reinterpret_cast<const Type*>(space_); + } + + inline Type* operator->() { return get(); } + inline const Type* operator->() const { return get(); } + + inline Type& operator*() { return *get(); } + inline const Type& operator*() const { return *get(); } + + // This is needed to get around the strict aliasing warning GCC generates. + inline void* space() { + return reinterpret_cast<void*>(space_); + } + + // You can pass up to four constructor arguments as arguments of Init(). + inline void Init() { + new(space()) Type; + } + + template <typename T1> + inline void Init(const T1& p1) { + new(space()) Type(p1); + } + + template <typename T1, typename T2> + inline void Init(const T1& p1, const T2& p2) { + new(space()) Type(p1, p2); + } + + template <typename T1, typename T2, typename T3> + inline void Init(const T1& p1, const T2& p2, const T3& p3) { + new(space()) Type(p1, p2, p3); + } + + template <typename T1, typename T2, typename T3, typename T4> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4) { + new(space()) Type(p1, p2, p3, p4); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5) { + new(space()) Type(p1, p2, p3, p4, p5); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6) { + new(space()) Type(p1, p2, p3, p4, p5, p6); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8, typename T9> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8, typename T9, typename T10> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9, const T10& p10) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8, typename T9, typename T10, + typename T11> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9, const T10& p10, const T11& p11) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11); + } + + inline void Destroy() { + get()->~Type(); + } + + private: + CERES_ALIGNED_CHAR_ARRAY(Type, 1) space_; +}; + +#undef CERES_ALIGNED_CHAR_ARRAY + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ diff --git a/extern/ceres/include/ceres/internal/numeric_diff.h b/extern/ceres/include/ceres/internal/numeric_diff.h new file mode 100644 index 00000000000..11e8275b1d3 --- /dev/null +++ b/extern/ceres/include/ceres/internal/numeric_diff.h @@ -0,0 +1,446 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// mierle@gmail.com (Keir Mierle) +// tbennun@gmail.com (Tal Ben-Nun) +// +// Finite differencing routines used by NumericDiffCostFunction. + +#ifndef CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ +#define CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ + +#include <cstring> + +#include "Eigen/Dense" +#include "Eigen/StdVector" +#include "ceres/cost_function.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/internal/variadic_evaluate.h" +#include "ceres/numeric_diff_options.h" +#include "ceres/types.h" +#include "glog/logging.h" + + +namespace ceres { +namespace internal { + +// Helper templates that allow evaluation of a variadic functor or a +// CostFunction object. +template <typename CostFunctor, + int N0, int N1, int N2, int N3, int N4, + int N5, int N6, int N7, int N8, int N9 > +bool EvaluateImpl(const CostFunctor* functor, + double const* const* parameters, + double* residuals, + const void* /* NOT USED */) { + return VariadicEvaluate<CostFunctor, + double, + N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call( + *functor, + parameters, + residuals); +} + +template <typename CostFunctor, + int N0, int N1, int N2, int N3, int N4, + int N5, int N6, int N7, int N8, int N9 > +bool EvaluateImpl(const CostFunctor* functor, + double const* const* parameters, + double* residuals, + const CostFunction* /* NOT USED */) { + return functor->Evaluate(parameters, residuals, NULL); +} + +// This is split from the main class because C++ doesn't allow partial template +// specializations for member functions. The alternative is to repeat the main +// class for differing numbers of parameters, which is also unfortunate. +template <typename CostFunctor, + NumericDiffMethodType kMethod, + int kNumResiduals, + int N0, int N1, int N2, int N3, int N4, + int N5, int N6, int N7, int N8, int N9, + int kParameterBlock, + int kParameterBlockSize> +struct NumericDiff { + // Mutates parameters but must restore them before return. + static bool EvaluateJacobianForParameterBlock( + const CostFunctor* functor, + const double* residuals_at_eval_point, + const NumericDiffOptions& options, + int num_residuals, + int parameter_block_index, + int parameter_block_size, + double **parameters, + double *jacobian) { + using Eigen::Map; + using Eigen::Matrix; + using Eigen::RowMajor; + using Eigen::ColMajor; + + const int num_residuals_internal = + (kNumResiduals != ceres::DYNAMIC ? kNumResiduals : num_residuals); + const int parameter_block_index_internal = + (kParameterBlock != ceres::DYNAMIC ? kParameterBlock : + parameter_block_index); + const int parameter_block_size_internal = + (kParameterBlockSize != ceres::DYNAMIC ? kParameterBlockSize : + parameter_block_size); + + typedef Matrix<double, kNumResiduals, 1> ResidualVector; + typedef Matrix<double, kParameterBlockSize, 1> ParameterVector; + + // The convoluted reasoning for choosing the Row/Column major + // ordering of the matrix is an artifact of the restrictions in + // Eigen that prevent it from creating RowMajor matrices with a + // single column. In these cases, we ask for a ColMajor matrix. + typedef Matrix<double, + kNumResiduals, + kParameterBlockSize, + (kParameterBlockSize == 1) ? ColMajor : RowMajor> + JacobianMatrix; + + Map<JacobianMatrix> parameter_jacobian(jacobian, + num_residuals_internal, + parameter_block_size_internal); + + Map<ParameterVector> x_plus_delta( + parameters[parameter_block_index_internal], + parameter_block_size_internal); + ParameterVector x(x_plus_delta); + ParameterVector step_size = x.array().abs() * + ((kMethod == RIDDERS) ? options.ridders_relative_initial_step_size : + options.relative_step_size); + + // It is not a good idea to make the step size arbitrarily + // small. This will lead to problems with round off and numerical + // instability when dividing by the step size. The general + // recommendation is to not go down below sqrt(epsilon). + double min_step_size = std::sqrt(std::numeric_limits<double>::epsilon()); + + // For Ridders' method, the initial step size is required to be large, + // thus ridders_relative_initial_step_size is used. + if (kMethod == RIDDERS) { + min_step_size = std::max(min_step_size, + options.ridders_relative_initial_step_size); + } + + // For each parameter in the parameter block, use finite differences to + // compute the derivative for that parameter. + FixedArray<double> temp_residual_array(num_residuals_internal); + FixedArray<double> residual_array(num_residuals_internal); + Map<ResidualVector> residuals(residual_array.get(), + num_residuals_internal); + + for (int j = 0; j < parameter_block_size_internal; ++j) { + const double delta = std::max(min_step_size, step_size(j)); + + if (kMethod == RIDDERS) { + if (!EvaluateRiddersJacobianColumn(functor, j, delta, + options, + num_residuals_internal, + parameter_block_size_internal, + x.data(), + residuals_at_eval_point, + parameters, + x_plus_delta.data(), + temp_residual_array.get(), + residual_array.get())) { + return false; + } + } else { + if (!EvaluateJacobianColumn(functor, j, delta, + num_residuals_internal, + parameter_block_size_internal, + x.data(), + residuals_at_eval_point, + parameters, + x_plus_delta.data(), + temp_residual_array.get(), + residual_array.get())) { + return false; + } + } + + parameter_jacobian.col(j).matrix() = residuals; + } + return true; + } + + static bool EvaluateJacobianColumn(const CostFunctor* functor, + int parameter_index, + double delta, + int num_residuals, + int parameter_block_size, + const double* x_ptr, + const double* residuals_at_eval_point, + double** parameters, + double* x_plus_delta_ptr, + double* temp_residuals_ptr, + double* residuals_ptr) { + using Eigen::Map; + using Eigen::Matrix; + + typedef Matrix<double, kNumResiduals, 1> ResidualVector; + typedef Matrix<double, kParameterBlockSize, 1> ParameterVector; + + Map<const ParameterVector> x(x_ptr, parameter_block_size); + Map<ParameterVector> x_plus_delta(x_plus_delta_ptr, + parameter_block_size); + + Map<ResidualVector> residuals(residuals_ptr, num_residuals); + Map<ResidualVector> temp_residuals(temp_residuals_ptr, num_residuals); + + // Mutate 1 element at a time and then restore. + x_plus_delta(parameter_index) = x(parameter_index) + delta; + + if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( + functor, parameters, residuals.data(), functor)) { + return false; + } + + // Compute this column of the jacobian in 3 steps: + // 1. Store residuals for the forward part. + // 2. Subtract residuals for the backward (or 0) part. + // 3. Divide out the run. + double one_over_delta = 1.0 / delta; + if (kMethod == CENTRAL || kMethod == RIDDERS) { + // Compute the function on the other side of x(parameter_index). + x_plus_delta(parameter_index) = x(parameter_index) - delta; + + if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( + functor, parameters, temp_residuals.data(), functor)) { + return false; + } + + residuals -= temp_residuals; + one_over_delta /= 2; + } else { + // Forward difference only; reuse existing residuals evaluation. + residuals -= + Map<const ResidualVector>(residuals_at_eval_point, + num_residuals); + } + + // Restore x_plus_delta. + x_plus_delta(parameter_index) = x(parameter_index); + + // Divide out the run to get slope. + residuals *= one_over_delta; + + return true; + } + + // This numeric difference implementation uses adaptive differentiation + // on the parameters to obtain the Jacobian matrix. The adaptive algorithm + // is based on Ridders' method for adaptive differentiation, which creates + // a Romberg tableau from varying step sizes and extrapolates the + // intermediate results to obtain the current computational error. + // + // References: + // C.J.F. Ridders, Accurate computation of F'(x) and F'(x) F"(x), Advances + // in Engineering Software (1978), Volume 4, Issue 2, April 1982, + // Pages 75-76, ISSN 0141-1195, + // http://dx.doi.org/10.1016/S0141-1195(82)80057-0. + static bool EvaluateRiddersJacobianColumn( + const CostFunctor* functor, + int parameter_index, + double delta, + const NumericDiffOptions& options, + int num_residuals, + int parameter_block_size, + const double* x_ptr, + const double* residuals_at_eval_point, + double** parameters, + double* x_plus_delta_ptr, + double* temp_residuals_ptr, + double* residuals_ptr) { + using Eigen::Map; + using Eigen::Matrix; + using Eigen::aligned_allocator; + + typedef Matrix<double, kNumResiduals, 1> ResidualVector; + typedef Matrix<double, kNumResiduals, Eigen::Dynamic> ResidualCandidateMatrix; + typedef Matrix<double, kParameterBlockSize, 1> ParameterVector; + + Map<const ParameterVector> x(x_ptr, parameter_block_size); + Map<ParameterVector> x_plus_delta(x_plus_delta_ptr, + parameter_block_size); + + Map<ResidualVector> residuals(residuals_ptr, num_residuals); + Map<ResidualVector> temp_residuals(temp_residuals_ptr, num_residuals); + + // In order for the algorithm to converge, the step size should be + // initialized to a value that is large enough to produce a significant + // change in the function. + // As the derivative is estimated, the step size decreases. + // By default, the step sizes are chosen so that the middle column + // of the Romberg tableau uses the input delta. + double current_step_size = delta * + pow(options.ridders_step_shrink_factor, + options.max_num_ridders_extrapolations / 2); + + // Double-buffering temporary differential candidate vectors + // from previous step size. + ResidualCandidateMatrix stepsize_candidates_a( + num_residuals, + options.max_num_ridders_extrapolations); + ResidualCandidateMatrix stepsize_candidates_b( + num_residuals, + options.max_num_ridders_extrapolations); + ResidualCandidateMatrix* current_candidates = &stepsize_candidates_a; + ResidualCandidateMatrix* previous_candidates = &stepsize_candidates_b; + + // Represents the computational error of the derivative. This variable is + // initially set to a large value, and is set to the difference between + // current and previous finite difference extrapolations. + // norm_error is supposed to decrease as the finite difference tableau + // generation progresses, serving both as an estimate for differentiation + // error and as a measure of differentiation numerical stability. + double norm_error = std::numeric_limits<double>::max(); + + // Loop over decreasing step sizes until: + // 1. Error is smaller than a given value (ridders_epsilon), + // 2. Maximal order of extrapolation reached, or + // 3. Extrapolation becomes numerically unstable. + for (int i = 0; i < options.max_num_ridders_extrapolations; ++i) { + // Compute the numerical derivative at this step size. + if (!EvaluateJacobianColumn(functor, parameter_index, current_step_size, + num_residuals, + parameter_block_size, + x.data(), + residuals_at_eval_point, + parameters, + x_plus_delta.data(), + temp_residuals.data(), + current_candidates->col(0).data())) { + // Something went wrong; bail. + return false; + } + + // Store initial results. + if (i == 0) { + residuals = current_candidates->col(0); + } + + // Shrink differentiation step size. + current_step_size /= options.ridders_step_shrink_factor; + + // Extrapolation factor for Richardson acceleration method (see below). + double richardson_factor = options.ridders_step_shrink_factor * + options.ridders_step_shrink_factor; + for (int k = 1; k <= i; ++k) { + // Extrapolate the various orders of finite differences using + // the Richardson acceleration method. + current_candidates->col(k) = + (richardson_factor * current_candidates->col(k - 1) - + previous_candidates->col(k - 1)) / (richardson_factor - 1.0); + + richardson_factor *= options.ridders_step_shrink_factor * + options.ridders_step_shrink_factor; + + // Compute the difference between the previous value and the current. + double candidate_error = std::max( + (current_candidates->col(k) - + current_candidates->col(k - 1)).norm(), + (current_candidates->col(k) - + previous_candidates->col(k - 1)).norm()); + + // If the error has decreased, update results. + if (candidate_error <= norm_error) { + norm_error = candidate_error; + residuals = current_candidates->col(k); + + // If the error is small enough, stop. + if (norm_error < options.ridders_epsilon) { + break; + } + } + } + + // After breaking out of the inner loop, declare convergence. + if (norm_error < options.ridders_epsilon) { + break; + } + + // Check to see if the current gradient estimate is numerically unstable. + // If so, bail out and return the last stable result. + if (i > 0) { + double tableau_error = (current_candidates->col(i) - + previous_candidates->col(i - 1)).norm(); + + // Compare current error to the chosen candidate's error. + if (tableau_error >= 2 * norm_error) { + break; + } + } + + std::swap(current_candidates, previous_candidates); + } + return true; + } +}; + +template <typename CostFunctor, + NumericDiffMethodType kMethod, + int kNumResiduals, + int N0, int N1, int N2, int N3, int N4, + int N5, int N6, int N7, int N8, int N9, + int kParameterBlock> +struct NumericDiff<CostFunctor, kMethod, kNumResiduals, + N0, N1, N2, N3, N4, N5, N6, N7, N8, N9, + kParameterBlock, 0> { + // Mutates parameters but must restore them before return. + static bool EvaluateJacobianForParameterBlock( + const CostFunctor* functor, + const double* residuals_at_eval_point, + const NumericDiffOptions& options, + const int num_residuals, + const int parameter_block_index, + const int parameter_block_size, + double **parameters, + double *jacobian) { + // Silence unused parameter compiler warnings. + (void)functor; + (void)residuals_at_eval_point; + (void)options; + (void)num_residuals; + (void)parameter_block_index; + (void)parameter_block_size; + (void)parameters; + (void)jacobian; + LOG(FATAL) << "Control should never reach here."; + return true; + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ diff --git a/extern/ceres/include/ceres/internal/port.h b/extern/ceres/include/ceres/internal/port.h new file mode 100644 index 00000000000..e57049dde4b --- /dev/null +++ b/extern/ceres/include/ceres/internal/port.h @@ -0,0 +1,76 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_PUBLIC_INTERNAL_PORT_H_ +#define CERES_PUBLIC_INTERNAL_PORT_H_ + +// This file needs to compile as c code. +#ifdef __cplusplus + +#include "ceres/internal/config.h" + +#if defined(CERES_TR1_MEMORY_HEADER) +#include <tr1/memory> +#else +#include <memory> +#endif + +namespace ceres { + +#if defined(CERES_TR1_SHARED_PTR) +using std::tr1::shared_ptr; +#else +using std::shared_ptr; +#endif + +} // namespace ceres + +#endif // __cplusplus + +// A macro to signal which functions and classes are exported when +// building a DLL with MSVC. +// +// Note that the ordering here is important, CERES_BUILDING_SHARED_LIBRARY +// is only defined locally when Ceres is compiled, it is never exported to +// users. However, in order that we do not have to configure config.h +// separately for building vs installing, if we are using MSVC and building +// a shared library, then both CERES_BUILDING_SHARED_LIBRARY and +// CERES_USING_SHARED_LIBRARY will be defined when Ceres is compiled. +// Hence it is important that the check for CERES_BUILDING_SHARED_LIBRARY +// happens first. +#if defined(_MSC_VER) && defined(CERES_BUILDING_SHARED_LIBRARY) +# define CERES_EXPORT __declspec(dllexport) +#elif defined(_MSC_VER) && defined(CERES_USING_SHARED_LIBRARY) +# define CERES_EXPORT __declspec(dllimport) +#else +# define CERES_EXPORT +#endif + +#endif // CERES_PUBLIC_INTERNAL_PORT_H_ diff --git a/extern/ceres/include/ceres/internal/reenable_warnings.h b/extern/ceres/include/ceres/internal/reenable_warnings.h new file mode 100644 index 00000000000..7e410259d64 --- /dev/null +++ b/extern/ceres/include/ceres/internal/reenable_warnings.h @@ -0,0 +1,38 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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. +// + +// This is not your usual header guard. See disable_warnings.h +#ifdef CERES_WARNINGS_DISABLED +#undef CERES_WARNINGS_DISABLED + +#ifdef _MSC_VER +#pragma warning( pop ) +#endif + +#endif // CERES_WARNINGS_DISABLED diff --git a/extern/ceres/include/ceres/internal/scoped_ptr.h b/extern/ceres/include/ceres/internal/scoped_ptr.h new file mode 100644 index 00000000000..fa0ac25a031 --- /dev/null +++ b/extern/ceres/include/ceres/internal/scoped_ptr.h @@ -0,0 +1,310 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: jorg@google.com (Jorg Brown) +// +// This is an implementation designed to match the anticipated future TR2 +// implementation of the scoped_ptr class, and its closely-related brethren, +// scoped_array, scoped_ptr_malloc, and make_scoped_ptr. + +#ifndef CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ +#define CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ + +#include <assert.h> +#include <stdlib.h> +#include <cstddef> +#include <algorithm> + +namespace ceres { +namespace internal { + +template <class C> class scoped_ptr; +template <class C, class Free> class scoped_ptr_malloc; +template <class C> class scoped_array; + +template <class C> +scoped_ptr<C> make_scoped_ptr(C *); + +// A scoped_ptr<T> is like a T*, except that the destructor of +// scoped_ptr<T> automatically deletes the pointer it holds (if +// any). That is, scoped_ptr<T> owns the T object that it points +// to. Like a T*, a scoped_ptr<T> may hold either NULL or a pointer to +// a T object. Also like T*, scoped_ptr<T> is thread-compatible, and +// once you dereference it, you get the threadsafety guarantees of T. +// +// The size of a scoped_ptr is small: sizeof(scoped_ptr<C>) == sizeof(C*) +template <class C> +class scoped_ptr { + public: + // The element type + typedef C element_type; + + // Constructor. Defaults to intializing with NULL. + // There is no way to create an uninitialized scoped_ptr. + // The input parameter must be allocated with new. + explicit scoped_ptr(C* p = NULL) : ptr_(p) { } + + // Destructor. If there is a C object, delete it. + // We don't need to test ptr_ == NULL because C++ does that for us. + ~scoped_ptr() { + enum { type_must_be_complete = sizeof(C) }; + delete ptr_; + } + + // Reset. Deletes the current owned object, if any. + // Then takes ownership of a new object, if given. + // this->reset(this->get()) works. + void reset(C* p = NULL) { + if (p != ptr_) { + enum { type_must_be_complete = sizeof(C) }; + delete ptr_; + ptr_ = p; + } + } + + // Accessors to get the owned object. + // operator* and operator-> will assert() if there is no current object. + C& operator*() const { + assert(ptr_ != NULL); + return *ptr_; + } + C* operator->() const { + assert(ptr_ != NULL); + return ptr_; + } + C* get() const { return ptr_; } + + // Comparison operators. + // These return whether a scoped_ptr and a raw pointer refer to + // the same object, not just to two different but equal objects. + bool operator==(const C* p) const { return ptr_ == p; } + bool operator!=(const C* p) const { return ptr_ != p; } + + // Swap two scoped pointers. + void swap(scoped_ptr& p2) { + C* tmp = ptr_; + ptr_ = p2.ptr_; + p2.ptr_ = tmp; + } + + // Release a pointer. + // The return value is the current pointer held by this object. + // If this object holds a NULL pointer, the return value is NULL. + // After this operation, this object will hold a NULL pointer, + // and will not own the object any more. + C* release() { + C* retVal = ptr_; + ptr_ = NULL; + return retVal; + } + + private: + C* ptr_; + + // google3 friend class that can access copy ctor (although if it actually + // calls a copy ctor, there will be a problem) see below + friend scoped_ptr<C> make_scoped_ptr<C>(C *p); + + // Forbid comparison of scoped_ptr types. If C2 != C, it totally doesn't + // make sense, and if C2 == C, it still doesn't make sense because you should + // never have the same object owned by two different scoped_ptrs. + template <class C2> bool operator==(scoped_ptr<C2> const& p2) const; + template <class C2> bool operator!=(scoped_ptr<C2> const& p2) const; + + // Disallow evil constructors + scoped_ptr(const scoped_ptr&); + void operator=(const scoped_ptr&); +}; + +// Free functions +template <class C> +inline void swap(scoped_ptr<C>& p1, scoped_ptr<C>& p2) { + p1.swap(p2); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_ptr<C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_ptr<const C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_ptr<C>& p2) { + return p1 != p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_ptr<const C>& p2) { + return p1 != p2.get(); +} + +template <class C> +scoped_ptr<C> make_scoped_ptr(C *p) { + // This does nothing but to return a scoped_ptr of the type that the passed + // pointer is of. (This eliminates the need to specify the name of T when + // making a scoped_ptr that is used anonymously/temporarily.) From an + // access control point of view, we construct an unnamed scoped_ptr here + // which we return and thus copy-construct. Hence, we need to have access + // to scoped_ptr::scoped_ptr(scoped_ptr const &). However, it is guaranteed + // that we never actually call the copy constructor, which is a good thing + // as we would call the temporary's object destructor (and thus delete p) + // if we actually did copy some object, here. + return scoped_ptr<C>(p); +} + +// scoped_array<C> is like scoped_ptr<C>, except that the caller must allocate +// with new [] and the destructor deletes objects with delete []. +// +// As with scoped_ptr<C>, a scoped_array<C> either points to an object +// or is NULL. A scoped_array<C> owns the object that it points to. +// scoped_array<T> is thread-compatible, and once you index into it, +// the returned objects have only the threadsafety guarantees of T. +// +// Size: sizeof(scoped_array<C>) == sizeof(C*) +template <class C> +class scoped_array { + public: + // The element type + typedef C element_type; + + // Constructor. Defaults to intializing with NULL. + // There is no way to create an uninitialized scoped_array. + // The input parameter must be allocated with new []. + explicit scoped_array(C* p = NULL) : array_(p) { } + + // Destructor. If there is a C object, delete it. + // We don't need to test ptr_ == NULL because C++ does that for us. + ~scoped_array() { + enum { type_must_be_complete = sizeof(C) }; + delete[] array_; + } + + // Reset. Deletes the current owned object, if any. + // Then takes ownership of a new object, if given. + // this->reset(this->get()) works. + void reset(C* p = NULL) { + if (p != array_) { + enum { type_must_be_complete = sizeof(C) }; + delete[] array_; + array_ = p; + } + } + + // Get one element of the current object. + // Will assert() if there is no current object, or index i is negative. + C& operator[](std::ptrdiff_t i) const { + assert(i >= 0); + assert(array_ != NULL); + return array_[i]; + } + + // Get a pointer to the zeroth element of the current object. + // If there is no current object, return NULL. + C* get() const { + return array_; + } + + // Comparison operators. + // These return whether a scoped_array and a raw pointer refer to + // the same array, not just to two different but equal arrays. + bool operator==(const C* p) const { return array_ == p; } + bool operator!=(const C* p) const { return array_ != p; } + + // Swap two scoped arrays. + void swap(scoped_array& p2) { + C* tmp = array_; + array_ = p2.array_; + p2.array_ = tmp; + } + + // Release an array. + // The return value is the current pointer held by this object. + // If this object holds a NULL pointer, the return value is NULL. + // After this operation, this object will hold a NULL pointer, + // and will not own the object any more. + C* release() { + C* retVal = array_; + array_ = NULL; + return retVal; + } + + private: + C* array_; + + // Forbid comparison of different scoped_array types. + template <class C2> bool operator==(scoped_array<C2> const& p2) const; + template <class C2> bool operator!=(scoped_array<C2> const& p2) const; + + // Disallow evil constructors + scoped_array(const scoped_array&); + void operator=(const scoped_array&); +}; + +// Free functions +template <class C> +inline void swap(scoped_array<C>& p1, scoped_array<C>& p2) { + p1.swap(p2); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_array<C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_array<const C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_array<C>& p2) { + return p1 != p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_array<const C>& p2) { + return p1 != p2.get(); +} + +// This class wraps the c library function free() in a class that can be +// passed as a template argument to scoped_ptr_malloc below. +class ScopedPtrMallocFree { + public: + inline void operator()(void* x) const { + free(x); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ diff --git a/extern/ceres/include/ceres/internal/variadic_evaluate.h b/extern/ceres/include/ceres/internal/variadic_evaluate.h new file mode 100644 index 00000000000..b3515b96d18 --- /dev/null +++ b/extern/ceres/include/ceres/internal/variadic_evaluate.h @@ -0,0 +1,194 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// mierle@gmail.com (Keir Mierle) + +#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ +#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ + +#include <stddef.h> + +#include "ceres/jet.h" +#include "ceres/types.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// This block of quasi-repeated code calls the user-supplied functor, which may +// take a variable number of arguments. This is accomplished by specializing the +// struct based on the size of the trailing parameters; parameters with 0 size +// are assumed missing. +template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, + int N5, int N6, int N7, int N8, int N9> +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + input[8], + input[9], + output); + } +}; + +template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, + int N5, int N6, int N7, int N8> +struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, N8, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + input[8], + output); + } +}; + +template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, + int N5, int N6, int N7> +struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + output); + } +}; + +template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, + int N5, int N6> +struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + output); + } +}; + +template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, + int N5> +struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, 0, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + output); + } +}; + +template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4> +struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0, 0, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + output); + } +}; + +template<typename Functor, typename T, int N0, int N1, int N2, int N3> +struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0, 0, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + output); + } +}; + +template<typename Functor, typename T, int N0, int N1, int N2> +struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0, 0, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + output); + } +}; + +template<typename Functor, typename T, int N0, int N1> +struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0, 0, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + output); + } +}; + +template<typename Functor, typename T, int N0> +struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0, 0, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + output); + } +}; + +// Template instantiation for dynamically-sized functors. +template<typename Functor, typename T> +struct VariadicEvaluate<Functor, T, ceres::DYNAMIC, ceres::DYNAMIC, + ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC, + ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC, + ceres::DYNAMIC, ceres::DYNAMIC> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input, output); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ diff --git a/extern/ceres/include/ceres/iteration_callback.h b/extern/ceres/include/ceres/iteration_callback.h new file mode 100644 index 00000000000..6bab00439c5 --- /dev/null +++ b/extern/ceres/include/ceres/iteration_callback.h @@ -0,0 +1,225 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// When an iteration callback is specified, Ceres calls the callback +// after each minimizer step (if the minimizer has not converged) and +// passes it an IterationSummary object, defined below. + +#ifndef CERES_PUBLIC_ITERATION_CALLBACK_H_ +#define CERES_PUBLIC_ITERATION_CALLBACK_H_ + +#include "ceres/types.h" +#include "ceres/internal/disable_warnings.h" + +namespace ceres { + +// This struct describes the state of the optimizer after each +// iteration of the minimization. +struct CERES_EXPORT IterationSummary { + IterationSummary() + : iteration(0), + step_is_valid(false), + step_is_nonmonotonic(false), + step_is_successful(false), + cost(0.0), + cost_change(0.0), + gradient_max_norm(0.0), + gradient_norm(0.0), + step_norm(0.0), + 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), + cumulative_time_in_seconds(0.0) {} + + // Current iteration number. + int32 iteration; + + // Step was numerically valid, i.e., all values are finite and the + // step reduces the value of the linearized model. + // + // Note: step_is_valid is false when iteration = 0. + bool step_is_valid; + + // Step did not reduce the value of the objective function + // sufficiently, but it was accepted because of the relaxed + // acceptance criterion used by the non-monotonic trust region + // algorithm. + // + // Note: step_is_nonmonotonic is false when iteration = 0; + bool step_is_nonmonotonic; + + // Whether or not the minimizer accepted this step or not. If the + // ordinary trust region algorithm is used, this means that the + // relative reduction in the objective function value was greater + // than Solver::Options::min_relative_decrease. However, if the + // non-monotonic trust region algorithm is used + // (Solver::Options:use_nonmonotonic_steps = true), then even if the + // relative decrease is not sufficient, the algorithm may accept the + // step and the step is declared successful. + // + // Note: step_is_successful is false when iteration = 0. + bool step_is_successful; + + // Value of the objective function. + double cost; + + // Change in the value of the objective function in this + // iteration. This can be positive or negative. + double cost_change; + + // Infinity norm of the gradient vector. + double gradient_max_norm; + + // 2-norm of the gradient vector. + double gradient_norm; + + // 2-norm of the size of the step computed by the optimization + // algorithm. + double step_norm; + + // For trust region algorithms, the ratio of the actual change in + // cost and the change in the cost of the linearized approximation. + double relative_decrease; + + // Size of the trust region at the end of the current iteration. For + // the Levenberg-Marquardt algorithm, the regularization parameter + // mu = 1.0 / trust_region_radius. + double trust_region_radius; + + // For the inexact step Levenberg-Marquardt algorithm, this is the + // relative accuracy with which the Newton(LM) step is solved. This + // number affects only the iterative solvers capable of solving + // linear systems inexactly. Factorization-based exact solvers + // ignore it. + double eta; + + // Step sized computed by the line search algorithm. + double step_size; + + // 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; + + // Time (in seconds) spent inside the trust region step solver. + double step_solver_time_in_seconds; + + // Time (in seconds) since the user called Solve(). + double cumulative_time_in_seconds; +}; + +// Interface for specifying callbacks that are executed at the end of +// each iteration of the Minimizer. The solver uses the return value +// of operator() to decide whether to continue solving or to +// terminate. The user can return three values. +// +// SOLVER_ABORT indicates that the callback detected an abnormal +// situation. The solver returns without updating the parameter blocks +// (unless Solver::Options::update_state_every_iteration is set +// true). Solver returns with Solver::Summary::termination_type set to +// USER_ABORT. +// +// SOLVER_TERMINATE_SUCCESSFULLY indicates that there is no need to +// optimize anymore (some user specified termination criterion has +// been met). Solver returns with Solver::Summary::termination_type +// set to USER_SUCCESS. +// +// SOLVER_CONTINUE indicates that the solver should continue +// optimizing. +// +// For example, the following Callback is used internally by Ceres to +// log the progress of the optimization. +// +// Callback for logging the state of the minimizer to STDERR or STDOUT +// depending on the user's preferences and logging level. +// +// class LoggingCallback : public IterationCallback { +// public: +// explicit LoggingCallback(bool log_to_stdout) +// : log_to_stdout_(log_to_stdout) {} +// +// ~LoggingCallback() {} +// +// CallbackReturnType operator()(const IterationSummary& summary) { +// const char* kReportRowFormat = +// "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " +// "rho:% 3.2e mu:% 3.2e eta:% 3.2e li:% 3d"; +// string output = StringPrintf(kReportRowFormat, +// summary.iteration, +// summary.cost, +// summary.cost_change, +// summary.gradient_max_norm, +// summary.step_norm, +// summary.relative_decrease, +// summary.trust_region_radius, +// summary.eta, +// summary.linear_solver_iterations); +// if (log_to_stdout_) { +// cout << output << endl; +// } else { +// VLOG(1) << output; +// } +// return SOLVER_CONTINUE; +// } +// +// private: +// const bool log_to_stdout_; +// }; +// +class CERES_EXPORT IterationCallback { + public: + virtual ~IterationCallback() {} + virtual CallbackReturnType operator()(const IterationSummary& summary) = 0; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_ITERATION_CALLBACK_H_ diff --git a/extern/ceres/include/ceres/jet.h b/extern/ceres/include/ceres/jet.h new file mode 100644 index 00000000000..a21fd7adb90 --- /dev/null +++ b/extern/ceres/include/ceres/jet.h @@ -0,0 +1,784 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A simple implementation of N-dimensional dual numbers, for automatically +// computing exact derivatives of functions. +// +// While a complete treatment of the mechanics of automatic differentation is +// beyond the scope of this header (see +// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the +// basic idea is to extend normal arithmetic with an extra element, "e," often +// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual +// numbers are extensions of the real numbers analogous to complex numbers: +// whereas complex numbers augment the reals by introducing an imaginary unit i +// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such +// that e^2 = 0. Dual numbers have two components: the "real" component and the +// "infinitesimal" component, generally written as x + y*e. Surprisingly, this +// leads to a convenient method for computing exact derivatives without needing +// to manipulate complicated symbolic expressions. +// +// For example, consider the function +// +// f(x) = x^2 , +// +// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20. +// Next, augument 10 with an infinitesimal to get: +// +// f(10 + e) = (10 + e)^2 +// = 100 + 2 * 10 * e + e^2 +// = 100 + 20 * e -+- +// -- | +// | +--- This is zero, since e^2 = 0 +// | +// +----------------- This is df/dx! +// +// Note that the derivative of f with respect to x is simply the infinitesimal +// component of the value of f(x + e). So, in order to take the derivative of +// any function, it is only necessary to replace the numeric "object" used in +// the function with one extended with infinitesimals. The class Jet, defined in +// this header, is one such example of this, where substitution is done with +// templates. +// +// To handle derivatives of functions taking multiple arguments, different +// infinitesimals are used, one for each variable to take the derivative of. For +// example, consider a scalar function of two scalar parameters x and y: +// +// f(x, y) = x^2 + x * y +// +// Following the technique above, to compute the derivatives df/dx and df/dy for +// f(1, 3) involves doing two evaluations of f, the first time replacing x with +// x + e, the second time replacing y with y + e. +// +// For df/dx: +// +// f(1 + e, y) = (1 + e)^2 + (1 + e) * 3 +// = 1 + 2 * e + 3 + 3 * e +// = 4 + 5 * e +// +// --> df/dx = 5 +// +// For df/dy: +// +// f(1, 3 + e) = 1^2 + 1 * (3 + e) +// = 1 + 3 + e +// = 4 + e +// +// --> df/dy = 1 +// +// To take the gradient of f with the implementation of dual numbers ("jets") in +// this file, it is necessary to create a single jet type which has components +// for the derivative in x and y, and passing them to a templated version of f: +// +// template<typename T> +// T f(const T &x, const T &y) { +// return x * x + x * y; +// } +// +// // The "2" means there should be 2 dual number components. +// Jet<double, 2> x(0); // Pick the 0th dual number for x. +// Jet<double, 2> y(1); // Pick the 1st dual number for y. +// Jet<double, 2> z = f(x, y); +// +// LOG(INFO) << "df/dx = " << z.v[0] +// << "df/dy = " << z.v[1]; +// +// Most users should not use Jet objects directly; a wrapper around Jet objects, +// which makes computing the derivative, gradient, or jacobian of templated +// functors simple, is in autodiff.h. Even autodiff.h should not be used +// directly; instead autodiff_cost_function.h is typically the file of interest. +// +// For the more mathematically inclined, this file implements first-order +// "jets". A 1st order jet is an element of the ring +// +// T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2 +// +// which essentially means that each jet consists of a "scalar" value 'a' from T +// and a 1st order perturbation vector 'v' of length N: +// +// x = a + \sum_i v[i] t_i +// +// A shorthand is to write an element as x = a + u, where u is the pertubation. +// Then, the main point about the arithmetic of jets is that the product of +// perturbations is zero: +// +// (a + u) * (b + v) = ab + av + bu + uv +// = ab + (av + bu) + 0 +// +// which is what operator* implements below. Addition is simpler: +// +// (a + u) + (b + v) = (a + b) + (u + v). +// +// The only remaining question is how to evaluate the function of a jet, for +// which we use the chain rule: +// +// f(a + u) = f(a) + f'(a) u +// +// where f'(a) is the (scalar) derivative of f at a. +// +// By pushing these things through sufficiently and suitably templated +// functions, we can do automatic differentiation. Just be sure to turn on +// function inlining and common-subexpression elimination, or it will be very +// slow! +// +// WARNING: Most Ceres users should not directly include this file or know the +// details of how jets work. Instead the suggested method for automatic +// derivatives is to use autodiff_cost_function.h, which is a wrapper around +// both jets.h and autodiff.h to make taking derivatives of cost functions for +// use in Ceres easier. + +#ifndef CERES_PUBLIC_JET_H_ +#define CERES_PUBLIC_JET_H_ + +#include <cmath> +#include <iosfwd> +#include <iostream> // NOLINT +#include <limits> +#include <string> + +#include "Eigen/Core" +#include "ceres/fpclassify.h" + +namespace ceres { + +template <typename T, int N> +struct Jet { + enum { DIMENSION = N }; + + // Default-construct "a" because otherwise this can lead to false errors about + // uninitialized uses when other classes relying on default constructed T + // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that + // the C++ standard mandates that e.g. default constructed doubles are + // initialized to 0.0; see sections 8.5 of the C++03 standard. + Jet() : a() { + v.setZero(); + } + + // Constructor from scalar: a + 0. + explicit Jet(const T& value) { + a = value; + v.setZero(); + } + + // Constructor from scalar plus variable: a + t_i. + Jet(const T& value, int k) { + a = value; + v.setZero(); + v[k] = T(1.0); + } + + // Constructor from scalar and vector part + // The use of Eigen::DenseBase allows Eigen expressions + // to be passed in without being fully evaluated until + // they are assigned to v + template<typename Derived> + EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived> &v) + : a(a), v(v) { + } + + // Compound operators + Jet<T, N>& operator+=(const Jet<T, N> &y) { + *this = *this + y; + return *this; + } + + Jet<T, N>& operator-=(const Jet<T, N> &y) { + *this = *this - y; + return *this; + } + + Jet<T, N>& operator*=(const Jet<T, N> &y) { + *this = *this * y; + return *this; + } + + Jet<T, N>& operator/=(const Jet<T, N> &y) { + *this = *this / y; + return *this; + } + + // The scalar part. + T a; + + // The infinitesimal part. + // + // Note the Eigen::DontAlign bit is needed here because this object + // gets allocated on the stack and as part of other arrays and + // structs. Forcing the right alignment there is the source of much + // pain and suffering. Even if that works, passing Jets around to + // functions by value has problems because the C++ ABI does not + // guarantee alignment for function arguments. + // + // Setting the DontAlign bit prevents Eigen from using SSE for the + // various operations on Jets. This is a small performance penalty + // since the AutoDiff code will still expose much of the code as + // statically sized loops to the compiler. But given the subtle + // issues that arise due to alignment, especially when dealing with + // multiple platforms, it seems to be a trade off worth making. + Eigen::Matrix<T, N, 1, Eigen::DontAlign> v; +}; + +// Unary + +template<typename T, int N> inline +Jet<T, N> const& operator+(const Jet<T, N>& f) { + return f; +} + +// TODO(keir): Try adding __attribute__((always_inline)) to these functions to +// see if it causes a performance increase. + +// Unary - +template<typename T, int N> inline +Jet<T, N> operator-(const Jet<T, N>&f) { + return Jet<T, N>(-f.a, -f.v); +} + +// Binary + +template<typename T, int N> inline +Jet<T, N> operator+(const Jet<T, N>& f, + const Jet<T, N>& g) { + return Jet<T, N>(f.a + g.a, f.v + g.v); +} + +// Binary + with a scalar: x + s +template<typename T, int N> inline +Jet<T, N> operator+(const Jet<T, N>& f, T s) { + return Jet<T, N>(f.a + s, f.v); +} + +// Binary + with a scalar: s + x +template<typename T, int N> inline +Jet<T, N> operator+(T s, const Jet<T, N>& f) { + return Jet<T, N>(f.a + s, f.v); +} + +// Binary - +template<typename T, int N> inline +Jet<T, N> operator-(const Jet<T, N>& f, + const Jet<T, N>& g) { + return Jet<T, N>(f.a - g.a, f.v - g.v); +} + +// Binary - with a scalar: x - s +template<typename T, int N> inline +Jet<T, N> operator-(const Jet<T, N>& f, T s) { + return Jet<T, N>(f.a - s, f.v); +} + +// Binary - with a scalar: s - x +template<typename T, int N> inline +Jet<T, N> operator-(T s, const Jet<T, N>& f) { + return Jet<T, N>(s - f.a, -f.v); +} + +// Binary * +template<typename T, int N> inline +Jet<T, N> operator*(const Jet<T, N>& f, + const Jet<T, N>& g) { + return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a); +} + +// Binary * with a scalar: x * s +template<typename T, int N> inline +Jet<T, N> operator*(const Jet<T, N>& f, T s) { + return Jet<T, N>(f.a * s, f.v * s); +} + +// Binary * with a scalar: s * x +template<typename T, int N> inline +Jet<T, N> operator*(T s, const Jet<T, N>& f) { + return Jet<T, N>(f.a * s, f.v * s); +} + +// Binary / +template<typename T, int N> inline +Jet<T, N> operator/(const Jet<T, N>& f, + const Jet<T, N>& g) { + // This uses: + // + // a + u (a + u)(b - v) (a + u)(b - v) + // ----- = -------------- = -------------- + // b + v (b + v)(b - v) b^2 + // + // which holds because v*v = 0. + const T g_a_inverse = T(1.0) / g.a; + const T f_a_by_g_a = f.a * g_a_inverse; + return Jet<T, N>(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse); +} + +// Binary / with a scalar: s / x +template<typename T, int N> inline +Jet<T, N> operator/(T s, const Jet<T, N>& g) { + const T minus_s_g_a_inverse2 = -s / (g.a * g.a); + return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2); +} + +// Binary / with a scalar: x / s +template<typename T, int N> inline +Jet<T, N> operator/(const Jet<T, N>& f, T s) { + const T s_inverse = 1.0 / s; + return Jet<T, N>(f.a * s_inverse, f.v * s_inverse); +} + +// Binary comparison operators for both scalars and jets. +#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ +template<typename T, int N> inline \ +bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \ + return f.a op g.a; \ +} \ +template<typename T, int N> inline \ +bool operator op(const T& s, const Jet<T, N>& g) { \ + return s op g.a; \ +} \ +template<typename T, int N> inline \ +bool operator op(const Jet<T, N>& f, const T& s) { \ + return f.a op s; \ +} +CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT +#undef CERES_DEFINE_JET_COMPARISON_OPERATOR + +// Pull some functions from namespace std. +// +// This is necessary because we want to use the same name (e.g. 'sqrt') for +// double-valued and Jet-valued functions, but we are not allowed to put +// Jet-valued functions inside namespace std. +// +// TODO(keir): Switch to "using". +inline double abs (double x) { return std::abs(x); } +inline double log (double x) { return std::log(x); } +inline double exp (double x) { return std::exp(x); } +inline double sqrt (double x) { return std::sqrt(x); } +inline double cos (double x) { return std::cos(x); } +inline double acos (double x) { return std::acos(x); } +inline double sin (double x) { return std::sin(x); } +inline double asin (double x) { return std::asin(x); } +inline 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); } + +// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. + +// abs(x + h) ~= x + h or -(x + h) +template <typename T, int N> inline +Jet<T, N> abs(const Jet<T, N>& f) { + return f.a < T(0.0) ? -f : f; +} + +// log(a + h) ~= log(a) + h / a +template <typename T, int N> inline +Jet<T, N> log(const Jet<T, N>& f) { + const T a_inverse = T(1.0) / f.a; + return Jet<T, N>(log(f.a), f.v * a_inverse); +} + +// exp(a + h) ~= exp(a) + exp(a) h +template <typename T, int N> inline +Jet<T, N> exp(const Jet<T, N>& f) { + const T tmp = exp(f.a); + return Jet<T, N>(tmp, tmp * f.v); +} + +// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) +template <typename T, int N> inline +Jet<T, N> sqrt(const Jet<T, N>& f) { + const T tmp = sqrt(f.a); + const T two_a_inverse = T(1.0) / (T(2.0) * tmp); + return Jet<T, N>(tmp, f.v * two_a_inverse); +} + +// cos(a + h) ~= cos(a) - sin(a) h +template <typename T, int N> inline +Jet<T, N> cos(const Jet<T, N>& f) { + return Jet<T, N>(cos(f.a), - sin(f.a) * f.v); +} + +// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h +template <typename T, int N> inline +Jet<T, N> acos(const Jet<T, N>& f) { + const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a); + return Jet<T, N>(acos(f.a), tmp * f.v); +} + +// sin(a + h) ~= sin(a) + cos(a) h +template <typename T, int N> inline +Jet<T, N> sin(const Jet<T, N>& f) { + return Jet<T, N>(sin(f.a), cos(f.a) * f.v); +} + +// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h +template <typename T, int N> inline +Jet<T, N> asin(const Jet<T, N>& f) { + const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); + return Jet<T, N>(asin(f.a), tmp * f.v); +} + +// 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) { + const T tan_a = tan(f.a); + const T tmp = T(1.0) + tan_a * tan_a; + return Jet<T, N>(tan_a, tmp * f.v); +} + +// 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) { + const T tmp = T(1.0) / (T(1.0) + f.a * f.a); + return Jet<T, N>(atan(f.a), tmp * f.v); +} + +// sinh(a + h) ~= sinh(a) + cosh(a) h +template <typename T, int N> inline +Jet<T, N> sinh(const Jet<T, N>& f) { + return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v); +} + +// cosh(a + h) ~= cosh(a) + sinh(a) h +template <typename T, int N> inline +Jet<T, N> cosh(const Jet<T, N>& f) { + return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v); +} + +// 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) { + const T tanh_a = tanh(f.a); + const T tmp = T(1.0) - tanh_a * tanh_a; + return Jet<T, N>(tanh_a, tmp * f.v); +} + +// Bessel functions of the first kind with integer order equal to 0, 1, n. +inline double BesselJ0(double x) { return j0(x); } +inline double BesselJ1(double x) { return j1(x); } +inline double BesselJn(int n, double x) { return jn(n, x); } + +// For the formulae of the derivatives of the Bessel functions see the book: +// Olver, Lozier, Boisvert, Clark, NIST Handbook of Mathematical Functions, +// Cambridge University Press 2010. +// +// Formulae are also available at http://dlmf.nist.gov + +// See formula http://dlmf.nist.gov/10.6#E3 +// j0(a + h) ~= j0(a) - j1(a) h +template <typename T, int N> inline +Jet<T, N> BesselJ0(const Jet<T, N>& f) { + return Jet<T, N>(BesselJ0(f.a), + -BesselJ1(f.a) * f.v); +} + +// See formula http://dlmf.nist.gov/10.6#E1 +// j1(a + h) ~= j1(a) + 0.5 ( j0(a) - j2(a) ) h +template <typename T, int N> inline +Jet<T, N> BesselJ1(const Jet<T, N>& f) { + return Jet<T, N>(BesselJ1(f.a), + T(0.5) * (BesselJ0(f.a) - BesselJn(2, f.a)) * f.v); +} + +// See formula http://dlmf.nist.gov/10.6#E1 +// j_n(a + h) ~= j_n(a) + 0.5 ( j_{n-1}(a) - j_{n+1}(a) ) h +template <typename T, int N> inline +Jet<T, N> BesselJn(int n, const Jet<T, N>& f) { + return Jet<T, N>(BesselJn(n, f.a), + T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v); +} + +// Jet Classification. It is not clear what the appropriate semantics are for +// these classifications. This picks that IsFinite and isnormal are "all" +// operations, i.e. all elements of the jet must be finite for the jet itself +// to be finite (or normal). For IsNaN and IsInfinite, the answer is less +// clear. This takes a "any" approach for IsNaN and IsInfinite such that if any +// part of a jet is nan or inf, then the entire jet is nan or inf. This leads +// to strange situations like a jet can be both IsInfinite and IsNaN, but in +// practice the "any" semantics are the most useful for e.g. checking that +// derivatives are sane. + +// The jet is finite if all parts of the jet are finite. +template <typename T, int N> inline +bool IsFinite(const Jet<T, N>& f) { + if (!IsFinite(f.a)) { + return false; + } + for (int i = 0; i < N; ++i) { + if (!IsFinite(f.v[i])) { + return false; + } + } + return true; +} + +// The jet is infinite if any part of the jet is infinite. +template <typename T, int N> inline +bool IsInfinite(const Jet<T, N>& f) { + if (IsInfinite(f.a)) { + return true; + } + for (int i = 0; i < N; i++) { + if (IsInfinite(f.v[i])) { + return true; + } + } + return false; +} + +// The jet is NaN if any part of the jet is NaN. +template <typename T, int N> inline +bool IsNaN(const Jet<T, N>& f) { + if (IsNaN(f.a)) { + return true; + } + for (int i = 0; i < N; ++i) { + if (IsNaN(f.v[i])) { + return true; + } + } + return false; +} + +// The jet is normal if all parts of the jet are normal. +template <typename T, int N> inline +bool IsNormal(const Jet<T, N>& f) { + if (!IsNormal(f.a)) { + return false; + } + for (int i = 0; i < N; ++i) { + if (!IsNormal(f.v[i])) { + return false; + } + } + return true; +} + +// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) +// +// In words: the rate of change of theta is 1/r times the rate of +// change of (x, y) in the positive angular direction. +template <typename T, int N> inline +Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { + // Note order of arguments: + // + // f = a + da + // g = b + db + + T const tmp = T(1.0) / (f.a * f.a + g.a * g.a); + return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v)); +} + + +// pow -- base is a differentiable function, exponent is a constant. +// (a+da)^p ~= a^p + p*a^(p-1) da +template <typename T, int N> inline +Jet<T, N> pow(const Jet<T, N>& f, double g) { + T const tmp = g * pow(f.a, g - T(1.0)); + return Jet<T, N>(pow(f.a, g), tmp * f.v); +} + +// pow -- base is a constant, exponent is a differentiable function. +// We have various special cases, see the comment for pow(Jet, Jet) for +// analysis: +// +// 1. For f > 0 we have: (f)^(g + dg) ~= f^g + f^g log(f) dg +// +// 2. For f == 0 and g > 0 we have: (f)^(g + dg) ~= f^g +// +// 3. For f < 0 and integer g we have: (f)^(g + dg) ~= f^g but if dg +// != 0, the derivatives are not defined and we return NaN. + +template <typename T, int N> inline +Jet<T, N> pow(double f, const Jet<T, N>& g) { + if (f == 0 && g.a > 0) { + // Handle case 2. + return Jet<T, N>(T(0.0)); + } + if (f < 0 && g.a == floor(g.a)) { + // Handle case 3. + Jet<T, N> ret(pow(f, g.a)); + for (int i = 0; i < N; i++) { + if (g.v[i] != T(0.0)) { + // Return a NaN when g.v != 0. + ret.v[i] = std::numeric_limits<T>::quiet_NaN(); + } + } + return ret; + } + // Handle case 1. + T const tmp = pow(f, g.a); + return Jet<T, N>(tmp, log(f) * tmp * g.v); +} + +// pow -- both base and exponent are differentiable functions. This has a +// variety of special cases that require careful handling. +// +// 1. For f > 0: +// (f + df)^(g + dg) ~= f^g + f^(g - 1) * (g * df + f * log(f) * dg) +// The numerical evaluation of f * log(f) for f > 0 is well behaved, even for +// extremely small values (e.g. 1e-99). +// +// 2. For f == 0 and g > 1: (f + df)^(g + dg) ~= 0 +// This cases is needed because log(0) can not be evaluated in the f > 0 +// expression. However the function f*log(f) is well behaved around f == 0 +// and its limit as f-->0 is zero. +// +// 3. For f == 0 and g == 1: (f + df)^(g + dg) ~= 0 + df +// +// 4. For f == 0 and 0 < g < 1: The value is finite but the derivatives are not. +// +// 5. For f == 0 and g < 0: The value and derivatives of f^g are not finite. +// +// 6. For f == 0 and g == 0: The C standard incorrectly defines 0^0 to be 1 +// "because there are applications that can exploit this definition". We +// (arbitrarily) decree that derivatives here will be nonfinite, since that +// is consistent with the behavior for f == 0, g < 0 and 0 < g < 1. +// Practically any definition could have been justified because mathematical +// consistency has been lost at this point. +// +// 7. For f < 0, g integer, dg == 0: (f + df)^(g + dg) ~= f^g + g * f^(g - 1) df +// This is equivalent to the case where f is a differentiable function and g +// is a constant (to first order). +// +// 8. For f < 0, g integer, dg != 0: The value is finite but the derivatives are +// not, because any change in the value of g moves us away from the point +// with a real-valued answer into the region with complex-valued answers. +// +// 9. For f < 0, g noninteger: The value and derivatives of f^g are not finite. + +template <typename T, int N> inline +Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { + if (f.a == 0 && g.a >= 1) { + // Handle cases 2 and 3. + if (g.a > 1) { + return Jet<T, N>(T(0.0)); + } + return f; + } + if (f.a < 0 && g.a == floor(g.a)) { + // Handle cases 7 and 8. + T const tmp = g.a * pow(f.a, g.a - T(1.0)); + Jet<T, N> ret(pow(f.a, g.a), tmp * f.v); + for (int i = 0; i < N; i++) { + if (g.v[i] != T(0.0)) { + // Return a NaN when g.v != 0. + ret.v[i] = std::numeric_limits<T>::quiet_NaN(); + } + } + return ret; + } + // Handle the remaining cases. For cases 4,5,6,9 we allow the log() function + // to generate -HUGE_VAL or NaN, since those cases result in a nonfinite + // derivative. + T const tmp1 = pow(f.a, g.a); + T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); + T const tmp3 = tmp1 * log(f.a); + return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v); +} + +// Define the helper functions Eigen needs to embed Jet types. +// +// NOTE(keir): machine_epsilon() and precision() are missing, because they don't +// work with nested template types (e.g. where the scalar is itself templated). +// Among other things, this means that decompositions of Jet's does not work, +// for example +// +// Matrix<Jet<T, N> ... > A, x, b; +// ... +// A.solve(b, &x) +// +// does not work and will fail with a strange compiler error. +// +// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we +// switch to 3.0, also add the rest of the specialization functionality. +template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x; } // NOLINT +template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x; } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_imag(const Jet<T, N>& ) { return Jet<T, N>(0.0); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_abs (const Jet<T, N>& x) { return fabs(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_abs2(const Jet<T, N>& x) { return x * x; } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_sqrt(const Jet<T, N>& x) { return sqrt(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) { return exp(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_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 +// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with +// strange compile errors. +template <typename T, int N> +inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) { + return s << "[" << z.a << " ; " << z.v.transpose() << "]"; +} + +} // namespace ceres + +namespace Eigen { + +// Creating a specialization of NumTraits enables placing Jet objects inside +// Eigen arrays, getting all the goodness of Eigen combined with autodiff. +template<typename T, int N> +struct NumTraits<ceres::Jet<T, N> > { + typedef ceres::Jet<T, N> Real; + typedef ceres::Jet<T, N> NonInteger; + typedef ceres::Jet<T, N> Nested; + + static typename ceres::Jet<T, N> dummy_precision() { + return ceres::Jet<T, N>(1e-12); + } + + static inline Real epsilon() { + return Real(std::numeric_limits<T>::epsilon()); + } + + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned, + ReadCost = 1, + AddCost = 1, + // For Jet types, multiplication is more expensive than addition. + MulCost = 3, + HasFloatingPoint = 1, + RequireInitialization = 1 + }; +}; + +} // namespace Eigen + +#endif // CERES_PUBLIC_JET_H_ diff --git a/extern/ceres/include/ceres/local_parameterization.h b/extern/ceres/include/ceres/local_parameterization.h new file mode 100644 index 00000000000..67633de309f --- /dev/null +++ b/extern/ceres/include/ceres/local_parameterization.h @@ -0,0 +1,301 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ +#define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ + +#include <vector> +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/internal/disable_warnings.h" + +namespace ceres { + +// Purpose: Sometimes parameter blocks x can overparameterize a problem +// +// min f(x) +// x +// +// In that case it is desirable to choose a parameterization for the +// block itself to remove the null directions of the cost. More +// generally, if x lies on a manifold of a smaller dimension than the +// ambient space that it is embedded in, then it is numerically and +// computationally more effective to optimize it using a +// parameterization that lives in the tangent space of that manifold +// at each point. +// +// For example, a sphere in three dimensions is a 2 dimensional +// manifold, embedded in a three dimensional space. At each point on +// the sphere, the plane tangent to it defines a two dimensional +// tangent space. For a cost function defined on this sphere, given a +// point x, moving in the direction normal to the sphere at that point +// is not useful. Thus a better way to do a local optimization is to +// optimize over two dimensional vector delta in the tangent space at +// that point and then "move" to the point x + delta, where the move +// operation involves projecting back onto the sphere. Doing so +// removes a redundent dimension from the optimization, making it +// numerically more robust and efficient. +// +// More generally we can define a function +// +// x_plus_delta = Plus(x, delta), +// +// where x_plus_delta has the same size as x, and delta is of size +// less than or equal to x. The function Plus, generalizes the +// definition of vector addition. Thus it satisfies the identify +// +// Plus(x, 0) = x, for all x. +// +// A trivial version of Plus is when delta is of the same size as x +// and +// +// Plus(x, delta) = x + delta +// +// A more interesting case if x is two dimensional vector, and the +// user wishes to hold the first coordinate constant. Then, delta is a +// scalar and Plus is defined as +// +// Plus(x, delta) = x + [0] * delta +// [1] +// +// An example that occurs commonly in Structure from Motion problems +// is when camera rotations are parameterized using Quaternion. There, +// it is useful only make updates orthogonal to that 4-vector defining +// the quaternion. One way to do this is to let delta be a 3 +// dimensional vector and define Plus to be +// +// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x +// +// The multiplication between the two 4-vectors on the RHS is the +// standard quaternion product. +// +// Given g and a point x, optimizing f can now be restated as +// +// min f(Plus(x, delta)) +// delta +// +// Given a solution delta to this problem, the optimal value is then +// given by +// +// x* = Plus(x, delta) +// +// The class LocalParameterization defines the function Plus and its +// Jacobian which is needed to compute the Jacobian of f w.r.t delta. +class CERES_EXPORT LocalParameterization { + public: + virtual ~LocalParameterization(); + + // Generalization of the addition operation, + // + // x_plus_delta = Plus(x, delta) + // + // with the condition that Plus(x, 0) = x. + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const = 0; + + // The jacobian of Plus(x, delta) w.r.t delta at delta = 0. + // + // jacobian is a row-major GlobalSize() x LocalSize() matrix. + virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; + + // local_matrix = global_matrix * jacobian + // + // global_matrix is a num_rows x GlobalSize row major matrix. + // local_matrix is a num_rows x LocalSize row major matrix. + // jacobian(x) is the matrix returned by ComputeJacobian at x. + // + // This is only used by GradientProblem. For most normal uses, it is + // okay to use the default implementation. + virtual bool MultiplyByJacobian(const double* x, + const int num_rows, + const double* global_matrix, + double* local_matrix) const; + + // Size of x. + virtual int GlobalSize() const = 0; + + // Size of delta. + virtual int LocalSize() const = 0; +}; + +// Some basic parameterizations + +// Identity Parameterization: Plus(x, delta) = x + delta +class CERES_EXPORT IdentityParameterization : public LocalParameterization { + public: + explicit IdentityParameterization(int size); + virtual ~IdentityParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual bool MultiplyByJacobian(const double* x, + const int num_cols, + const double* global_matrix, + double* local_matrix) const; + virtual int GlobalSize() const { return size_; } + virtual int LocalSize() const { return size_; } + + private: + const int size_; +}; + +// Hold a subset of the parameters inside a parameter block constant. +class CERES_EXPORT SubsetParameterization : public LocalParameterization { + public: + explicit SubsetParameterization(int size, + const std::vector<int>& constant_parameters); + virtual ~SubsetParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual bool MultiplyByJacobian(const double* x, + const int num_cols, + const double* global_matrix, + double* local_matrix) const; + virtual int GlobalSize() const { + return static_cast<int>(constancy_mask_.size()); + } + virtual int LocalSize() const { return local_size_; } + + private: + const int local_size_; + std::vector<char> constancy_mask_; +}; + +// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x +// with * being the quaternion multiplication operator. Here we assume +// that the first element of the quaternion vector is the real (cos +// theta) part. +class CERES_EXPORT QuaternionParameterization : public LocalParameterization { + public: + virtual ~QuaternionParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual int GlobalSize() const { return 4; } + virtual int LocalSize() const { return 3; } +}; + + +// This provides a parameterization for homogeneous vectors which are commonly +// used in Structure for Motion problems. One example where they are used is +// in representing points whose triangulation is ill-conditioned. Here +// it is advantageous to use an over-parameterization since homogeneous vectors +// can represent points at infinity. +// +// The plus operator is defined as +// Plus(x, delta) = +// [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x +// with * defined as an operator which applies the update orthogonal to x to +// remain on the sphere. We assume that the last element of x is the scalar +// component. The size of the homogeneous vector is required to be greater than +// 1. +class CERES_EXPORT HomogeneousVectorParameterization : + public LocalParameterization { + public: + explicit HomogeneousVectorParameterization(int size); + virtual ~HomogeneousVectorParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual int GlobalSize() const { return size_; } + virtual int LocalSize() const { return size_ - 1; } + + private: + const int size_; +}; + +// Construct a local parameterization by taking the Cartesian product +// of a number of other local parameterizations. This is useful, when +// a parameter block is the cartesian product of two or more +// manifolds. For example the parameters of a camera consist of a +// rotation and a translation, i.e., SO(3) x R^3. +// +// Currently this class supports taking the cartesian product of up to +// four local parameterizations. +// +// Example usage: +// +// ProductParameterization product_param(new QuaterionionParameterization(), +// new IdentityParameterization(3)); +// +// is the local parameterization for a rigid transformation, where the +// rotation is represented using a quaternion. +class CERES_EXPORT ProductParameterization : public LocalParameterization { + public: + // + // NOTE: All the constructors take ownership of the input local + // parameterizations. + // + ProductParameterization(LocalParameterization* local_param1, + LocalParameterization* local_param2); + + ProductParameterization(LocalParameterization* local_param1, + LocalParameterization* local_param2, + LocalParameterization* local_param3); + + ProductParameterization(LocalParameterization* local_param1, + LocalParameterization* local_param2, + LocalParameterization* local_param3, + LocalParameterization* local_param4); + + virtual ~ProductParameterization(); + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual int GlobalSize() const { return global_size_; } + virtual int LocalSize() const { return local_size_; } + + private: + void Init(); + + std::vector<LocalParameterization*> local_params_; + int local_size_; + int global_size_; + int buffer_size_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ diff --git a/extern/ceres/include/ceres/loss_function.h b/extern/ceres/include/ceres/loss_function.h new file mode 100644 index 00000000000..0512c135143 --- /dev/null +++ b/extern/ceres/include/ceres/loss_function.h @@ -0,0 +1,428 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// The LossFunction interface is the way users describe how residuals +// are converted to cost terms for the overall problem cost function. +// For the exact manner in which loss functions are converted to the +// overall cost for a problem, see problem.h. +// +// For least squares problem where there are no outliers and standard +// squared loss is expected, it is not necessary to create a loss +// function; instead passing a NULL to the problem when adding +// residuals implies a standard squared loss. +// +// For least squares problems where the minimization may encounter +// input terms that contain outliers, that is, completely bogus +// measurements, it is important to use a loss function that reduces +// their associated penalty. +// +// Consider a structure from motion problem. The unknowns are 3D +// points and camera parameters, and the measurements are image +// coordinates describing the expected reprojected position for a +// point in a camera. For example, we want to model the geometry of a +// street scene with fire hydrants and cars, observed by a moving +// camera with unknown parameters, and the only 3D points we care +// about are the pointy tippy-tops of the fire hydrants. Our magic +// image processing algorithm, which is responsible for producing the +// measurements that are input to Ceres, has found and matched all +// such tippy-tops in all image frames, except that in one of the +// frame it mistook a car's headlight for a hydrant. If we didn't do +// anything special (i.e. if we used a basic quadratic loss), the +// residual for the erroneous measurement will result in extreme error +// due to the quadratic nature of squared loss. This results in the +// entire solution getting pulled away from the optimimum to reduce +// the large error that would otherwise be attributed to the wrong +// measurement. +// +// Using a robust loss function, the cost for large residuals is +// reduced. In the example above, this leads to outlier terms getting +// downweighted so they do not overly influence the final solution. +// +// What cost function is best? +// +// In general, there isn't a principled way to select a robust loss +// function. The authors suggest starting with a non-robust cost, then +// only experimenting with robust loss functions if standard squared +// loss doesn't work. + +#ifndef CERES_PUBLIC_LOSS_FUNCTION_H_ +#define CERES_PUBLIC_LOSS_FUNCTION_H_ + +#include "glog/logging.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" +#include "ceres/internal/disable_warnings.h" + +namespace ceres { + +class CERES_EXPORT LossFunction { + public: + virtual ~LossFunction() {} + + // For a residual vector with squared 2-norm 'sq_norm', this method + // is required to fill in the value and derivatives of the loss + // function (rho in this example): + // + // out[0] = rho(sq_norm), + // out[1] = rho'(sq_norm), + // out[2] = rho''(sq_norm), + // + // Here the convention is that the contribution of a term to the + // cost function is given by 1/2 rho(s), where + // + // s = ||residuals||^2. + // + // Calling the method with a negative value of 's' is an error and + // the implementations are not required to handle that case. + // + // Most sane choices of rho() satisfy: + // + // rho(0) = 0, + // rho'(0) = 1, + // rho'(s) < 1 in outlier region, + // rho''(s) < 0 in outlier region, + // + // so that they mimic the least squares cost for small residuals. + virtual void Evaluate(double sq_norm, double out[3]) const = 0; +}; + +// Some common implementations follow below. +// +// Note: in the region of interest (i.e. s < 3) we have: +// TrivialLoss >= HuberLoss >= SoftLOneLoss >= CauchyLoss + + +// This corresponds to no robustification. +// +// rho(s) = s +// +// At s = 0: rho = [0, 1, 0]. +// +// It is not normally necessary to use this, as passing NULL for the +// loss function when building the problem accomplishes the same +// thing. +class CERES_EXPORT TrivialLoss : public LossFunction { + public: + virtual void Evaluate(double, double*) const; +}; + +// Scaling +// ------- +// Given one robustifier +// s -> rho(s) +// one can change the length scale at which robustification takes +// place, by adding a scale factor 'a' as follows: +// +// s -> a^2 rho(s / a^2). +// +// The first and second derivatives are: +// +// s -> rho'(s / a^2), +// s -> (1 / a^2) rho''(s / a^2), +// +// but the behaviour near s = 0 is the same as the original function, +// i.e. +// +// rho(s) = s + higher order terms, +// a^2 rho(s / a^2) = s + higher order terms. +// +// The scalar 'a' should be positive. +// +// The reason for the appearance of squaring is that 'a' is in the +// units of the residual vector norm whereas 's' is a squared +// norm. For applications it is more convenient to specify 'a' than +// its square. The commonly used robustifiers below are described in +// un-scaled format (a = 1) but their implementations work for any +// non-zero value of 'a'. + +// Huber. +// +// rho(s) = s for s <= 1, +// rho(s) = 2 sqrt(s) - 1 for s >= 1. +// +// At s = 0: rho = [0, 1, 0]. +// +// The scaling parameter 'a' corresponds to 'delta' on this page: +// http://en.wikipedia.org/wiki/Huber_Loss_Function +class CERES_EXPORT HuberLoss : public LossFunction { + public: + explicit HuberLoss(double a) : a_(a), b_(a * a) { } + virtual void Evaluate(double, double*) const; + + private: + const double a_; + // b = a^2. + const double b_; +}; + +// Soft L1, similar to Huber but smooth. +// +// rho(s) = 2 (sqrt(1 + s) - 1). +// +// At s = 0: rho = [0, 1, -1/2]. +class CERES_EXPORT SoftLOneLoss : public LossFunction { + public: + explicit SoftLOneLoss(double a) : b_(a * a), c_(1 / b_) { } + virtual void Evaluate(double, double*) const; + + private: + // b = a^2. + const double b_; + // c = 1 / a^2. + const double c_; +}; + +// Inspired by the Cauchy distribution +// +// rho(s) = log(1 + s). +// +// At s = 0: rho = [0, 1, -1]. +class CERES_EXPORT CauchyLoss : public LossFunction { + public: + explicit CauchyLoss(double a) : b_(a * a), c_(1 / b_) { } + virtual void Evaluate(double, double*) const; + + private: + // b = a^2. + const double b_; + // c = 1 / a^2. + const double c_; +}; + +// Loss that is capped beyond a certain level using the arc-tangent function. +// The scaling parameter 'a' determines the level where falloff occurs. +// For costs much smaller than 'a', the loss function is linear and behaves like +// TrivialLoss, and for values much larger than 'a' the value asymptotically +// approaches the constant value of a * PI / 2. +// +// rho(s) = a atan(s / a). +// +// At s = 0: rho = [0, 1, 0]. +class CERES_EXPORT ArctanLoss : public LossFunction { + public: + explicit ArctanLoss(double a) : a_(a), b_(1 / (a * a)) { } + virtual void Evaluate(double, double*) const; + + private: + const double a_; + // b = 1 / a^2. + const double b_; +}; + +// Loss function that maps to approximately zero cost in a range around the +// origin, and reverts to linear in error (quadratic in cost) beyond this range. +// The tolerance parameter 'a' sets the nominal point at which the +// transition occurs, and the transition size parameter 'b' sets the nominal +// distance over which most of the transition occurs. Both a and b must be +// greater than zero, and typically b will be set to a fraction of a. +// The slope rho'[s] varies smoothly from about 0 at s <= a - b to +// about 1 at s >= a + b. +// +// The term is computed as: +// +// rho(s) = b log(1 + exp((s - a) / b)) - c0. +// +// where c0 is chosen so that rho(0) == 0 +// +// c0 = b log(1 + exp(-a / b) +// +// This has the following useful properties: +// +// rho(s) == 0 for s = 0 +// rho'(s) ~= 0 for s << a - b +// rho'(s) ~= 1 for s >> a + b +// rho''(s) > 0 for all s +// +// In addition, all derivatives are continuous, and the curvature is +// concentrated in the range a - b to a + b. +// +// At s = 0: rho = [0, ~0, ~0]. +class CERES_EXPORT TolerantLoss : public LossFunction { + public: + explicit TolerantLoss(double a, double b); + virtual void Evaluate(double, double*) const; + + private: + const double a_, b_, c_; +}; + +// This is the Tukey biweight loss function which aggressively +// attempts to suppress large errors. +// +// The term is computed as: +// +// rho(s) = a^2 / 6 * (1 - (1 - s / a^2)^3 ) for s <= a^2, +// rho(s) = a^2 / 6 for s > a^2. +// +// At s = 0: rho = [0, 0.5, -1 / a^2] +class CERES_EXPORT TukeyLoss : public ceres::LossFunction { + public: + explicit TukeyLoss(double a) : a_squared_(a * a) { } + virtual void Evaluate(double, double*) const; + + private: + const double a_squared_; +}; + +// Composition of two loss functions. The error is the result of first +// evaluating g followed by f to yield the composition f(g(s)). +// The loss functions must not be NULL. +class CERES_EXPORT ComposedLoss : public LossFunction { + public: + explicit ComposedLoss(const LossFunction* f, Ownership ownership_f, + const LossFunction* g, Ownership ownership_g); + virtual ~ComposedLoss(); + virtual void Evaluate(double, double*) const; + + private: + internal::scoped_ptr<const LossFunction> f_, g_; + const Ownership ownership_f_, ownership_g_; +}; + +// The discussion above has to do with length scaling: it affects the space +// in which s is measured. Sometimes you want to simply scale the output +// value of the robustifier. For example, you might want to weight +// different error terms differently (e.g., weight pixel reprojection +// errors differently from terrain errors). +// +// If rho is the wrapped robustifier, then this simply outputs +// s -> a * rho(s) +// +// The first and second derivatives are, not surprisingly +// s -> a * rho'(s) +// s -> a * rho''(s) +// +// Since we treat the a NULL Loss function as the Identity loss +// function, rho = NULL is a valid input and will result in the input +// being scaled by a. This provides a simple way of implementing a +// scaled ResidualBlock. +class CERES_EXPORT ScaledLoss : public LossFunction { + public: + // Constructs a ScaledLoss wrapping another loss function. Takes + // ownership of the wrapped loss function or not depending on the + // ownership parameter. + ScaledLoss(const LossFunction* rho, double a, Ownership ownership) : + rho_(rho), a_(a), ownership_(ownership) { } + + virtual ~ScaledLoss() { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + } + virtual void Evaluate(double, double*) const; + + private: + internal::scoped_ptr<const LossFunction> rho_; + const double a_; + const Ownership ownership_; + CERES_DISALLOW_COPY_AND_ASSIGN(ScaledLoss); +}; + +// Sometimes after the optimization problem has been constructed, we +// wish to mutate the scale of the loss function. For example, when +// performing estimation from data which has substantial outliers, +// convergence can be improved by starting out with a large scale, +// optimizing the problem and then reducing the scale. This can have +// better convergence behaviour than just using a loss function with a +// small scale. +// +// This templated class allows the user to implement a loss function +// whose scale can be mutated after an optimization problem has been +// constructed. +// +// Since we treat the a NULL Loss function as the Identity loss +// function, rho = NULL is a valid input. +// +// Example usage +// +// Problem problem; +// +// // Add parameter blocks +// +// CostFunction* cost_function = +// new AutoDiffCostFunction < UW_Camera_Mapper, 2, 9, 3>( +// 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; +// Solger::Summary summary; +// +// Solve(options, &problem, &summary) +// +// loss_function->Reset(new HuberLoss(1.0), TAKE_OWNERSHIP); +// +// Solve(options, &problem, &summary) +// +class CERES_EXPORT LossFunctionWrapper : public LossFunction { + public: + LossFunctionWrapper(LossFunction* rho, Ownership ownership) + : rho_(rho), ownership_(ownership) { + } + + virtual ~LossFunctionWrapper() { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + } + + virtual void Evaluate(double sq_norm, double out[3]) const { + if (rho_.get() == NULL) { + out[0] = sq_norm; + out[1] = 1.0; + out[2] = 0.0; + } + else { + rho_->Evaluate(sq_norm, out); + } + } + + void Reset(LossFunction* rho, Ownership ownership) { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + rho_.reset(rho); + ownership_ = ownership; + } + + private: + internal::scoped_ptr<const LossFunction> rho_; + Ownership ownership_; + CERES_DISALLOW_COPY_AND_ASSIGN(LossFunctionWrapper); +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_LOSS_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/normal_prior.h b/extern/ceres/include/ceres/normal_prior.h new file mode 100644 index 00000000000..cd98b4c846b --- /dev/null +++ b/extern/ceres/include/ceres/normal_prior.h @@ -0,0 +1,78 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Cost term that implements a prior on a parameter block using a +// normal distribution. + +#ifndef CERES_PUBLIC_NORMAL_PRIOR_H_ +#define CERES_PUBLIC_NORMAL_PRIOR_H_ + +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/disable_warnings.h" + +namespace ceres { + +// Implements a cost function of the form +// +// cost(x) = ||A(x - b)||^2 +// +// where, the matrix A and the vector b are fixed and x is the +// variable. In case the user is interested in implementing a cost +// function of the form +// +// cost(x) = (x - mu)^T S^{-1} (x - mu) +// +// where, mu is a vector and S is a covariance matrix, then, A = +// S^{-1/2}, i.e the matrix A is the square root of the inverse of the +// covariance, also known as the stiffness matrix. There are however +// no restrictions on the shape of A. It is free to be rectangular, +// which would be the case if the covariance matrix S is rank +// deficient. + +class CERES_EXPORT NormalPrior: public CostFunction { + public: + // Check that the number of rows in the vector b are the same as the + // number of columns in the matrix A, crash otherwise. + NormalPrior(const Matrix& A, const Vector& b); + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const; + private: + Matrix A_; + Vector b_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_NORMAL_PRIOR_H_ diff --git a/extern/ceres/include/ceres/numeric_diff_cost_function.h b/extern/ceres/include/ceres/numeric_diff_cost_function.h new file mode 100644 index 00000000000..fa96078df02 --- /dev/null +++ b/extern/ceres/include/ceres/numeric_diff_cost_function.h @@ -0,0 +1,342 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Create CostFunctions as needed by the least squares framework with jacobians +// computed via numeric (a.k.a. finite) differentiation. For more details see +// http://en.wikipedia.org/wiki/Numerical_differentiation. +// +// To get 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. +// 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 +// transposition, and k is a constant. The form of this error, which is the +// difference between a constant and an expression, is a common pattern in least +// squares problems. For example, the value x'y might be the model expectation +// for a series of measurements, where there is an instance of the cost function +// for each measurement k. +// +// The actual cost added to the total problem is e^2, or (k - x'k)^2; however, +// the squaring is implicitly done by the optimization framework. +// +// To write an numerically-differentiable cost function for the above model, first +// define the object +// +// class MyScalarCostFunctor { +// MyScalarCostFunctor(double k): k_(k) {} +// +// bool operator()(const double* const x, +// const double* const y, +// double* residuals) const { +// residuals[0] = k_ - x[0] * y[0] + x[1] * y[1]; +// return true; +// } +// +// private: +// double k_; +// }; +// +// Note that in the declaration of operator() the input parameters x +// and y come first, and are passed as const pointers to arrays of +// doubles. If there were three input parameters, then the third input +// parameter would come after y. The output is always the last +// parameter, and is also a pointer to an array. In the example above, +// the residual is a scalar, so only residuals[0] is set. +// +// Then given this class definition, the numerically differentiated +// cost function with central differences used for computing the +// derivative can be constructed as follows. +// +// 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 -------------------------+ +// +// 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 +// a 1-dimensional output from two arguments, both 2-dimensional. +// +// NumericDiffCostFunction also supports cost functions with a +// runtime-determined number of residuals. For example: +// +// CostFunction* cost_function +// = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>( +// new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^ +// TAKE_OWNERSHIP, | | | +// runtime_number_of_residuals); <----+ | | | +// | | | | +// | | | | +// Actual number of residuals ------+ | | | +// Indicate dynamic number of residuals --------------------+ | | +// Dimension of x ------------------------------------------------+ | +// Dimension of y ---------------------------------------------------+ +// +// The framework can currently accommodate cost functions of up to 10 +// independent variables, and there is no limit on the dimensionality +// of each of them. +// +// The central difference method is considerably more accurate at the cost of +// twice as many function evaluations than forward difference. Consider using +// central differences begin with, and only after that works, trying forward +// difference to improve performance. +// +// WARNING #1: A common beginner's error when first using +// NumericDiffCostFunction is to get the sizing wrong. In particular, +// there is a tendency to set the template parameters to (dimension of +// residual, number of parameters) instead of passing a dimension +// parameter for *every parameter*. In the example above, that would +// be <MyScalarCostFunctor, 1, 2>, which is missing the last '2' +// argument. Please be careful when setting the size parameters. +// +//////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////// +// +// ALTERNATE INTERFACE +// +// For a variety of reasons, including compatibility with legacy code, +// NumericDiffCostFunction can also take CostFunction objects as +// input. The following describes how. +// +// 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 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 +// function, even though it could be implemented only in terms of the +// virtual CostFunction interface. +// +// The numerically differentiated version of a cost function for a cost function +// can be constructed as follows: +// +// CostFunction* cost_function +// = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>( +// new MyCostFunction(...), TAKE_OWNERSHIP); +// +// where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8 +// respectively. Look at the tests for a more detailed example. +// +// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives. + +#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ + +#include "Eigen/Dense" +#include "ceres/cost_function.h" +#include "ceres/internal/numeric_diff.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/numeric_diff_options.h" +#include "ceres/sized_cost_function.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +template <typename CostFunctor, + NumericDiffMethodType method = CENTRAL, + int kNumResiduals = 0, // Number of residuals, or ceres::DYNAMIC + int N0 = 0, // Number of parameters in block 0. + int N1 = 0, // Number of parameters in block 1. + int N2 = 0, // Number of parameters in block 2. + int N3 = 0, // Number of parameters in block 3. + int N4 = 0, // Number of parameters in block 4. + int N5 = 0, // Number of parameters in block 5. + int N6 = 0, // Number of parameters in block 6. + int N7 = 0, // Number of parameters in block 7. + int N8 = 0, // Number of parameters in block 8. + int N9 = 0> // Number of parameters in block 9. +class NumericDiffCostFunction + : public SizedCostFunction<kNumResiduals, + N0, N1, N2, N3, N4, + N5, N6, N7, N8, N9> { + public: + NumericDiffCostFunction( + CostFunctor* functor, + Ownership ownership = TAKE_OWNERSHIP, + int num_residuals = kNumResiduals, + const NumericDiffOptions& options = NumericDiffOptions()) + : functor_(functor), + ownership_(ownership), + options_(options) { + if (kNumResiduals == DYNAMIC) { + SizedCostFunction<kNumResiduals, + N0, N1, N2, N3, N4, + N5, N6, N7, N8, N9> + ::set_num_residuals(num_residuals); + } + } + + // Deprecated. New users should avoid using this constructor. Instead, use the + // constructor with NumericDiffOptions. + NumericDiffCostFunction(CostFunctor* functor, + Ownership ownership, + int num_residuals, + const double relative_step_size) + :functor_(functor), + ownership_(ownership), + options_() { + LOG(WARNING) << "This constructor is deprecated and will be removed in " + "a future version. Please use the NumericDiffOptions " + "constructor instead."; + + if (kNumResiduals == DYNAMIC) { + SizedCostFunction<kNumResiduals, + N0, N1, N2, N3, N4, + N5, N6, N7, N8, N9> + ::set_num_residuals(num_residuals); + } + + options_.relative_step_size = relative_step_size; + } + + ~NumericDiffCostFunction() { + if (ownership_ != TAKE_OWNERSHIP) { + functor_.release(); + } + } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + using internal::FixedArray; + using internal::NumericDiff; + + const int kNumParameters = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; + const int kNumParameterBlocks = + (N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) + + (N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0); + + // Get the function value (residuals) at the the point to evaluate. + if (!internal::EvaluateImpl<CostFunctor, + N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( + functor_.get(), + parameters, + residuals, + functor_.get())) { + return false; + } + + if (jacobians == NULL) { + return true; + } + + // Create a copy of the parameters which will get mutated. + FixedArray<double> parameters_copy(kNumParameters); + FixedArray<double*> parameters_reference_copy(kNumParameterBlocks); + + parameters_reference_copy[0] = parameters_copy.get(); + if (N1) parameters_reference_copy[1] = parameters_reference_copy[0] + N0; + if (N2) parameters_reference_copy[2] = parameters_reference_copy[1] + N1; + if (N3) parameters_reference_copy[3] = parameters_reference_copy[2] + N2; + if (N4) parameters_reference_copy[4] = parameters_reference_copy[3] + N3; + 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 (N8) parameters_reference_copy[8] = parameters_reference_copy[7] + N7; + if (N9) parameters_reference_copy[9] = parameters_reference_copy[8] + N8; + +#define CERES_COPY_PARAMETER_BLOCK(block) \ + if (N ## block) memcpy(parameters_reference_copy[block], \ + parameters[block], \ + sizeof(double) * N ## block); // NOLINT + + CERES_COPY_PARAMETER_BLOCK(0); + CERES_COPY_PARAMETER_BLOCK(1); + CERES_COPY_PARAMETER_BLOCK(2); + CERES_COPY_PARAMETER_BLOCK(3); + CERES_COPY_PARAMETER_BLOCK(4); + CERES_COPY_PARAMETER_BLOCK(5); + CERES_COPY_PARAMETER_BLOCK(6); + CERES_COPY_PARAMETER_BLOCK(7); + CERES_COPY_PARAMETER_BLOCK(8); + CERES_COPY_PARAMETER_BLOCK(9); + +#undef CERES_COPY_PARAMETER_BLOCK + +#define CERES_EVALUATE_JACOBIAN_FOR_BLOCK(block) \ + if (N ## block && jacobians[block] != NULL) { \ + if (!NumericDiff<CostFunctor, \ + method, \ + kNumResiduals, \ + N0, N1, N2, N3, N4, N5, N6, N7, N8, N9, \ + block, \ + N ## block >::EvaluateJacobianForParameterBlock( \ + functor_.get(), \ + residuals, \ + options_, \ + SizedCostFunction<kNumResiduals, \ + N0, N1, N2, N3, N4, \ + N5, N6, N7, N8, N9>::num_residuals(), \ + block, \ + N ## block, \ + parameters_reference_copy.get(), \ + jacobians[block])) { \ + return false; \ + } \ + } + + CERES_EVALUATE_JACOBIAN_FOR_BLOCK(0); + CERES_EVALUATE_JACOBIAN_FOR_BLOCK(1); + CERES_EVALUATE_JACOBIAN_FOR_BLOCK(2); + CERES_EVALUATE_JACOBIAN_FOR_BLOCK(3); + CERES_EVALUATE_JACOBIAN_FOR_BLOCK(4); + CERES_EVALUATE_JACOBIAN_FOR_BLOCK(5); + CERES_EVALUATE_JACOBIAN_FOR_BLOCK(6); + CERES_EVALUATE_JACOBIAN_FOR_BLOCK(7); + CERES_EVALUATE_JACOBIAN_FOR_BLOCK(8); + CERES_EVALUATE_JACOBIAN_FOR_BLOCK(9); + +#undef CERES_EVALUATE_JACOBIAN_FOR_BLOCK + + return true; + } + + private: + internal::scoped_ptr<CostFunctor> functor_; + Ownership ownership_; + NumericDiffOptions options_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/numeric_diff_options.h b/extern/ceres/include/ceres/numeric_diff_options.h new file mode 100644 index 00000000000..119c8a86596 --- /dev/null +++ b/extern/ceres/include/ceres/numeric_diff_options.h @@ -0,0 +1,79 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: tbennun@gmail.com (Tal Ben-Nun) +// + +#ifndef CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_ +#define CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_ + +namespace ceres { + +// Options pertaining to numeric differentiation (e.g., convergence criteria, +// step sizes). +struct CERES_EXPORT NumericDiffOptions { + NumericDiffOptions() { + relative_step_size = 1e-6; + ridders_relative_initial_step_size = 1e-2; + max_num_ridders_extrapolations = 10; + ridders_epsilon = 1e-12; + ridders_step_shrink_factor = 2.0; + } + + // Numeric differentiation step size (multiplied by parameter block's + // order of magnitude). If parameters are close to zero, the step size + // is set to sqrt(machine_epsilon). + double relative_step_size; + + // Initial step size for Ridders adaptive numeric differentiation (multiplied + // by parameter block's order of magnitude). + // If parameters are close to zero, Ridders' method sets the step size + // directly to this value. This parameter is separate from + // "relative_step_size" in order to set a different default value. + // + // Note: For Ridders' method to converge, the step size should be initialized + // to a value that is large enough to produce a significant change in the + // function. As the derivative is estimated, the step size decreases. + double ridders_relative_initial_step_size; + + // Maximal number of adaptive extrapolations (sampling) in Ridders' method. + int max_num_ridders_extrapolations; + + // Convergence criterion on extrapolation error for Ridders adaptive + // differentiation. The available error estimation methods are defined in + // NumericDiffErrorType and set in the "ridders_error_method" field. + double ridders_epsilon; + + // The factor in which to shrink the step size with each extrapolation in + // Ridders' method. + double ridders_step_shrink_factor; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_ diff --git a/extern/ceres/include/ceres/ordered_groups.h b/extern/ceres/include/ceres/ordered_groups.h new file mode 100644 index 00000000000..aa1bd3a7da1 --- /dev/null +++ b/extern/ceres/include/ceres/ordered_groups.h @@ -0,0 +1,208 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_ORDERED_GROUPS_H_ +#define CERES_PUBLIC_ORDERED_GROUPS_H_ + +#include <map> +#include <set> +#include <vector> +#include "ceres/internal/port.h" +#include "glog/logging.h" + +namespace ceres { + +// A class for storing and manipulating an ordered collection of +// groups/sets with the following semantics: +// +// Group ids are non-negative integer values. Elements are any type +// that can serve as a key in a map or an element of a set. +// +// An element can only belong to one group at a time. A group may +// contain an arbitrary number of elements. +// +// Groups are ordered by their group id. +template <typename T> +class OrderedGroups { + public: + // Add an element to a group. If a group with this id does not + // exist, one is created. This method can be called any number of + // times for the same element. Group ids should be non-negative + // numbers. + // + // Return value indicates if adding the element was a success. + bool AddElementToGroup(const T element, const int group) { + if (group < 0) { + return false; + } + + typename std::map<T, int>::const_iterator it = + element_to_group_.find(element); + if (it != element_to_group_.end()) { + if (it->second == group) { + // Element is already in the right group, nothing to do. + return true; + } + + group_to_elements_[it->second].erase(element); + if (group_to_elements_[it->second].size() == 0) { + group_to_elements_.erase(it->second); + } + } + + element_to_group_[element] = group; + group_to_elements_[group].insert(element); + return true; + } + + void Clear() { + group_to_elements_.clear(); + element_to_group_.clear(); + } + + // Remove the element, no matter what group it is in. Return value + // indicates if the element was actually removed. + bool Remove(const T element) { + const int current_group = GroupId(element); + if (current_group < 0) { + return false; + } + + group_to_elements_[current_group].erase(element); + + if (group_to_elements_[current_group].size() == 0) { + // If the group is empty, then get rid of it. + group_to_elements_.erase(current_group); + } + + element_to_group_.erase(element); + return true; + } + + // Bulk remove elements. The return value indicates the number of + // elements successfully removed. + int Remove(const std::vector<T>& elements) { + if (NumElements() == 0 || elements.size() == 0) { + return 0; + } + + int num_removed = 0; + for (int i = 0; i < elements.size(); ++i) { + num_removed += Remove(elements[i]); + } + return num_removed; + } + + // Reverse the order of the groups in place. + void Reverse() { + if (NumGroups() == 0) { + return; + } + + typename std::map<int, std::set<T> >::reverse_iterator it = + group_to_elements_.rbegin(); + std::map<int, std::set<T> > new_group_to_elements; + new_group_to_elements[it->first] = it->second; + + int new_group_id = it->first + 1; + for (++it; it != group_to_elements_.rend(); ++it) { + for (typename std::set<T>::const_iterator element_it = it->second.begin(); + element_it != it->second.end(); + ++element_it) { + element_to_group_[*element_it] = new_group_id; + } + new_group_to_elements[new_group_id] = it->second; + new_group_id++; + } + + group_to_elements_.swap(new_group_to_elements); + } + + // Return the group id for the element. If the element is not a + // member of any group, return -1. + int GroupId(const T element) const { + typename std::map<T, int>::const_iterator it = + element_to_group_.find(element); + if (it == element_to_group_.end()) { + return -1; + } + return it->second; + } + + bool IsMember(const T element) const { + typename std::map<T, int>::const_iterator it = + element_to_group_.find(element); + return (it != element_to_group_.end()); + } + + // This function always succeeds, i.e., implicitly there exists a + // group for every integer. + int GroupSize(const int group) const { + typename std::map<int, std::set<T> >::const_iterator it = + group_to_elements_.find(group); + return (it == group_to_elements_.end()) ? 0 : it->second.size(); + } + + int NumElements() const { + return element_to_group_.size(); + } + + // Number of groups with one or more elements. + int NumGroups() const { + return group_to_elements_.size(); + } + + // The first group with one or more elements. Calling this when + // there are no groups with non-zero elements will result in a + // crash. + int MinNonZeroGroup() const { + CHECK_NE(NumGroups(), 0); + return group_to_elements_.begin()->first; + } + + const std::map<int, std::set<T> >& group_to_elements() const { + return group_to_elements_; + } + + const std::map<T, int>& element_to_group() const { + return element_to_group_; + } + + private: + std::map<int, std::set<T> > group_to_elements_; + std::map<T, int> element_to_group_; +}; + +// Typedef for the most commonly used version of OrderedGroups. +typedef OrderedGroups<double*> ParameterBlockOrdering; + +} // namespace ceres + +#endif // CERES_PUBLIC_ORDERED_GROUP_H_ diff --git a/extern/ceres/include/ceres/problem.h b/extern/ceres/include/ceres/problem.h new file mode 100644 index 00000000000..409274c62c2 --- /dev/null +++ b/extern/ceres/include/ceres/problem.h @@ -0,0 +1,481 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) +// +// The Problem object is used to build and hold least squares problems. + +#ifndef CERES_PUBLIC_PROBLEM_H_ +#define CERES_PUBLIC_PROBLEM_H_ + +#include <cstddef> +#include <map> +#include <set> +#include <vector> + +#include "glog/logging.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" +#include "ceres/internal/disable_warnings.h" + + +namespace ceres { + +class CostFunction; +class LossFunction; +class LocalParameterization; +class Solver; +struct CRSMatrix; + +namespace internal { +class Preprocessor; +class ProblemImpl; +class ParameterBlock; +class ResidualBlock; +} // namespace internal + +// A ResidualBlockId is an opaque handle clients can use to remove residual +// blocks from a Problem after adding them. +typedef internal::ResidualBlock* ResidualBlockId; + +// A class to represent non-linear least squares problems. Such +// problems have a cost function that is a sum of error terms (known +// as "residuals"), where each residual is a function of some subset +// of the parameters. The cost function takes the form +// +// N 1 +// SUM --- loss( || r_i1, r_i2,..., r_ik ||^2 ), +// i=1 2 +// +// where +// +// r_ij is residual number i, component j; the residual is a +// function of some subset of the parameters x1...xk. For +// example, in a structure from motion problem a residual +// might be the difference between a measured point in an +// image and the reprojected position for the matching +// camera, point pair. The residual would have two +// components, error in x and error in y. +// +// loss(y) is the loss function; for example, squared error or +// Huber L1 loss. If loss(y) = y, then the cost function is +// non-robustified least squares. +// +// This class is specifically designed to address the important subset +// of "sparse" least squares problems, where each component of the +// residual depends only on a small number number of parameters, even +// though the total number of residuals and parameters may be very +// large. This property affords tremendous gains in scale, allowing +// efficient solving of large problems that are otherwise +// inaccessible. +// +// The canonical example of a sparse least squares problem is +// "structure-from-motion" (SFM), where the parameters are points and +// cameras, and residuals are reprojection errors. Typically a single +// residual will depend only on 9 parameters (3 for the point, 6 for +// the camera). +// +// To create a least squares problem, use the AddResidualBlock() and +// AddParameterBlock() methods, documented below. Here is an example least +// squares problem containing 3 parameter blocks of sizes 3, 4 and 5 +// respectively and two residual terms of size 2 and 6: +// +// double x1[] = { 1.0, 2.0, 3.0 }; +// double x2[] = { 1.0, 2.0, 3.0, 5.0 }; +// double x3[] = { 1.0, 2.0, 3.0, 6.0, 7.0 }; +// +// Problem problem; +// +// problem.AddResidualBlock(new MyUnaryCostFunction(...), x1); +// problem.AddResidualBlock(new MyBinaryCostFunction(...), x2, x3); +// +// Please see cost_function.h for details of the CostFunction object. +class CERES_EXPORT Problem { + public: + struct CERES_EXPORT Options { + Options() + : cost_function_ownership(TAKE_OWNERSHIP), + loss_function_ownership(TAKE_OWNERSHIP), + local_parameterization_ownership(TAKE_OWNERSHIP), + enable_fast_removal(false), + disable_all_safety_checks(false) {} + + // These flags control whether the Problem object owns the cost + // functions, loss functions, and parameterizations passed into + // the Problem. If set to TAKE_OWNERSHIP, then the problem object + // will delete the corresponding cost or loss functions on + // destruction. The destructor is careful to delete the pointers + // only once, since sharing cost/loss/parameterizations is + // allowed. + Ownership cost_function_ownership; + Ownership loss_function_ownership; + Ownership local_parameterization_ownership; + + // If true, trades memory for faster RemoveResidualBlock() and + // RemoveParameterBlock() operations. + // + // By default, RemoveParameterBlock() and RemoveResidualBlock() take time + // proportional to the size of the entire problem. If you only ever remove + // parameters or residuals from the problem occassionally, this might be + // acceptable. However, if you have memory to spare, enable this option to + // make RemoveParameterBlock() take time proportional to the number of + // residual blocks that depend on it, and RemoveResidualBlock() take (on + // average) constant time. + // + // The increase in memory usage is twofold: an additonal hash set per + // parameter block containing all the residuals that depend on the parameter + // block; and a hash set in the problem containing all residuals. + bool enable_fast_removal; + + // By default, Ceres performs a variety of safety checks when constructing + // the problem. There is a small but measurable performance penalty to + // these checks, typically around 5% of construction time. If you are sure + // your problem construction is correct, and 5% of the problem construction + // time is truly an overhead you want to avoid, then you can set + // disable_all_safety_checks to true. + // + // WARNING: Do not set this to true, unless you are absolutely sure of what + // you are doing. + bool disable_all_safety_checks; + }; + + // The default constructor is equivalent to the + // invocation Problem(Problem::Options()). + Problem(); + explicit Problem(const Options& options); + + ~Problem(); + + // Add a residual block to the overall cost function. The cost + // function carries with it information about the sizes of the + // parameter blocks it expects. The function checks that these match + // the sizes of the parameter blocks listed in parameter_blocks. The + // program aborts if a mismatch is detected. loss_function can be + // NULL, in which case the cost of the term is just the squared norm + // of the residuals. + // + // The user has the option of explicitly adding the parameter blocks + // using AddParameterBlock. This causes additional correctness + // checking; however, AddResidualBlock implicitly adds the parameter + // blocks if they are not present, so calling AddParameterBlock + // explicitly is not required. + // + // The Problem object by default takes ownership of the + // cost_function and loss_function pointers. These objects remain + // live for the life of the Problem object. If the user wishes to + // keep control over the destruction of these objects, then they can + // do this by setting the corresponding enums in the Options struct. + // + // Note: Even though the Problem takes ownership of cost_function + // and loss_function, it does not preclude the user from re-using + // them in another residual block. The destructor takes care to call + // delete on each cost_function or loss_function pointer only once, + // regardless of how many residual blocks refer to them. + // + // Example usage: + // + // double x1[] = {1.0, 2.0, 3.0}; + // double x2[] = {1.0, 2.0, 5.0, 6.0}; + // double x3[] = {3.0, 6.0, 2.0, 5.0, 1.0}; + // + // Problem problem; + // + // problem.AddResidualBlock(new MyUnaryCostFunction(...), NULL, x1); + // problem.AddResidualBlock(new MyBinaryCostFunction(...), NULL, x2, x1); + // + ResidualBlockId AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + const std::vector<double*>& parameter_blocks); + + // Convenience methods for adding residuals with a small number of + // parameters. This is the common case. Instead of specifying the + // parameter block arguments as a vector, list them as pointers. + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5, + double* x6); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5, + double* x6, double* x7); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5, + double* x6, double* x7, double* x8); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5, + double* x6, double* x7, double* x8, + double* x9); + + // Add a parameter block with appropriate size to the problem. + // Repeated calls with the same arguments are ignored. Repeated + // calls with the same double pointer but a different size results + // in undefined behaviour. + void AddParameterBlock(double* values, int size); + + // Add a parameter block with appropriate size and parameterization + // to the problem. Repeated calls with the same arguments are + // ignored. Repeated calls with the same double pointer but a + // different size results in undefined behaviour. + void AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization); + + // Remove a parameter block from the problem. The parameterization of the + // parameter block, if it exists, will persist until the deletion of the + // problem (similar to cost/loss functions in residual block removal). Any + // residual blocks that depend on the parameter are also removed, as + // described above in RemoveResidualBlock(). + // + // If Problem::Options::enable_fast_removal is true, then the + // removal is fast (almost constant time). Otherwise, removing a parameter + // block will incur a scan of the entire Problem object. + // + // WARNING: Removing a residual or parameter block will destroy the implicit + // ordering, rendering the jacobian or residuals returned from the solver + // uninterpretable. If you depend on the evaluated jacobian, do not use + // remove! This may change in a future release. + void RemoveParameterBlock(double* values); + + // Remove a residual block from the problem. Any parameters that the residual + // block depends on are not removed. The cost and loss functions for the + // residual block will not get deleted immediately; won't happen until the + // problem itself is deleted. + // + // WARNING: Removing a residual or parameter block will destroy the implicit + // ordering, rendering the jacobian or residuals returned from the solver + // uninterpretable. If you depend on the evaluated jacobian, do not use + // remove! This may change in a future release. + void RemoveResidualBlock(ResidualBlockId residual_block); + + // Hold the indicated parameter block constant during optimization. + void SetParameterBlockConstant(double* values); + + // Allow the indicated parameter block to vary during optimization. + void SetParameterBlockVariable(double* values); + + // Set the local parameterization for one of the parameter blocks. + // The local_parameterization is owned by the Problem by default. It + // is acceptable to set the same parameterization for multiple + // parameters; the destructor is careful to delete local + // parameterizations only once. The local parameterization can only + // be set once per parameter, and cannot be changed once set. + void SetParameterization(double* values, + LocalParameterization* local_parameterization); + + // Get the local parameterization object associated with this + // parameter block. If there is no parameterization object + // associated then NULL is returned. + const LocalParameterization* GetParameterization(double* values) const; + + // Set the lower/upper bound for the parameter with position "index". + void SetParameterLowerBound(double* values, int index, double lower_bound); + void SetParameterUpperBound(double* values, int index, double upper_bound); + + // Number of parameter blocks in the problem. Always equals + // parameter_blocks().size() and parameter_block_sizes().size(). + int NumParameterBlocks() const; + + // The size of the parameter vector obtained by summing over the + // sizes of all the parameter blocks. + int NumParameters() const; + + // Number of residual blocks in the problem. Always equals + // residual_blocks().size(). + int NumResidualBlocks() const; + + // The size of the residual vector obtained by summing over the + // sizes of all of the residual blocks. + int NumResiduals() const; + + // The size of the parameter block. + 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(const double* values) const; + + // Is the given parameter block present in this problem or not? + bool HasParameterBlock(const double* values) const; + + // Fills the passed parameter_blocks vector with pointers to the + // parameter blocks currently in the problem. After this call, + // parameter_block.size() == NumParameterBlocks. + void GetParameterBlocks(std::vector<double*>* parameter_blocks) const; + + // Fills the passed residual_blocks vector with pointers to the + // residual blocks currently in the problem. After this call, + // residual_blocks.size() == NumResidualBlocks. + void GetResidualBlocks(std::vector<ResidualBlockId>* residual_blocks) const; + + // Get all the parameter blocks that depend on the given residual block. + void GetParameterBlocksForResidualBlock( + const ResidualBlockId residual_block, + std::vector<double*>* parameter_blocks) const; + + // Get the CostFunction for the given residual block. + const CostFunction* GetCostFunctionForResidualBlock( + const ResidualBlockId residual_block) const; + + // Get the LossFunction for the given residual block. Returns NULL + // if no loss function is associated with this residual block. + const LossFunction* GetLossFunctionForResidualBlock( + const ResidualBlockId residual_block) const; + + // Get all the residual blocks that depend on the given parameter block. + // + // If Problem::Options::enable_fast_removal is true, then + // getting the residual blocks is fast and depends only on the number of + // residual blocks. Otherwise, getting the residual blocks for a parameter + // block will incur a scan of the entire Problem object. + void GetResidualBlocksForParameterBlock( + const double* values, + std::vector<ResidualBlockId>* residual_blocks) const; + + // Options struct to control Problem::Evaluate. + struct EvaluateOptions { + EvaluateOptions() + : apply_loss_function(true), + num_threads(1) { + } + + // The set of parameter blocks for which evaluation should be + // performed. This vector determines the order that parameter + // blocks occur in the gradient vector and in the columns of the + // jacobian matrix. If parameter_blocks is empty, then it is + // assumed to be equal to vector containing ALL the parameter + // blocks. Generally speaking the parameter blocks will occur in + // the order in which they were added to the problem. But, this + // may change if the user removes any parameter blocks from the + // problem. + // + // NOTE: This vector should contain the same pointers as the ones + // used to add parameter blocks to the Problem. These parameter + // block should NOT point to new memory locations. Bad things will + // happen otherwise. + std::vector<double*> parameter_blocks; + + // The set of residual blocks to evaluate. This vector determines + // the order in which the residuals occur, and how the rows of the + // jacobian are ordered. If residual_blocks is empty, then it is + // assumed to be equal to the vector containing ALL the residual + // blocks. Generally speaking the residual blocks will occur in + // the order in which they were added to the problem. But, this + // may change if the user removes any residual blocks from the + // problem. + std::vector<ResidualBlockId> residual_blocks; + + // 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. This is of use for example if the user wishes to + // analyse the solution quality by studying the distribution of + // residuals before and after the solve. + bool apply_loss_function; + + int num_threads; + }; + + // Evaluate Problem. Any of the output pointers can be NULL. Which + // residual blocks and parameter blocks are used is controlled by + // the EvaluateOptions struct above. + // + // Note 1: The evaluation will use the values stored in the memory + // locations pointed to by the parameter block pointers used at the + // time of the construction of the problem. i.e., + // + // Problem problem; + // double x = 1; + // problem.AddResidualBlock(new MyCostFunction, NULL, &x); + // + // double cost = 0.0; + // problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL); + // + // The cost is evaluated at x = 1. If you wish to evaluate the + // problem at x = 2, then + // + // x = 2; + // problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL); + // + // is the way to do so. + // + // Note 2: If no local parameterizations are used, then the size of + // the gradient vector (and the number of columns in the jacobian) + // is the sum of the sizes of all the parameter blocks. If a + // parameter block has a local parameterization, then it contributes + // "LocalSize" entries to the gradient vector (and the number of + // columns in the jacobian). + bool Evaluate(const EvaluateOptions& options, + double* cost, + std::vector<double>* residuals, + std::vector<double>* gradient, + CRSMatrix* jacobian); + + private: + friend class Solver; + friend class Covariance; + internal::scoped_ptr<internal::ProblemImpl> problem_impl_; + CERES_DISALLOW_COPY_AND_ASSIGN(Problem); +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_PROBLEM_H_ diff --git a/extern/ceres/include/ceres/rotation.h b/extern/ceres/include/ceres/rotation.h new file mode 100644 index 00000000000..e9496d772e4 --- /dev/null +++ b/extern/ceres/include/ceres/rotation.h @@ -0,0 +1,629 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Templated functions for manipulating rotations. The templated +// functions are useful when implementing functors for automatic +// differentiation. +// +// In the following, the Quaternions are laid out as 4-vectors, thus: +// +// q[0] scalar part. +// q[1] coefficient of i. +// q[2] coefficient of j. +// q[3] coefficient of k. +// +// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j. + +#ifndef CERES_PUBLIC_ROTATION_H_ +#define CERES_PUBLIC_ROTATION_H_ + +#include <algorithm> +#include <cmath> +#include <limits> +#include "glog/logging.h" + +namespace ceres { + +// Trivial wrapper to index linear arrays as matrices, given a fixed +// column and row stride. When an array "T* array" is wrapped by a +// +// (const) MatrixAdapter<T, row_stride, col_stride> M" +// +// the expression M(i, j) is equivalent to +// +// arrary[i * row_stride + j * col_stride] +// +// Conversion functions to and from rotation matrices accept +// MatrixAdapters to permit using row-major and column-major layouts, +// and rotation matrices embedded in larger matrices (such as a 3x4 +// projection matrix). +template <typename T, int row_stride, int col_stride> +struct MatrixAdapter; + +// Convenience functions to create a MatrixAdapter that treats the +// array pointed to by "pointer" as a 3x3 (contiguous) column-major or +// row-major matrix. +template <typename T> +MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer); + +template <typename T> +MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer); + +// Convert a value in combined axis-angle representation to a quaternion. +// The value angle_axis is a triple whose norm is an angle in radians, +// and whose direction is aligned with the axis of rotation, +// and quaternion is a 4-tuple that will contain the resulting quaternion. +// The implementation may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template<typename T> +void AngleAxisToQuaternion(const T* angle_axis, T* quaternion); + +// Convert a quaternion to the equivalent combined axis-angle representation. +// The value quaternion must be a unit quaternion - it is not normalized first, +// and angle_axis will be filled with a value whose norm is the angle of +// rotation in radians, and whose direction is the axis of rotation. +// The implemention may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template<typename T> +void QuaternionToAngleAxis(const T* quaternion, T* angle_axis); + +// Conversions between 3x3 rotation matrix (in column major order) and +// quaternion rotation representations. Templated for use with +// autodifferentiation. +template <typename T> +void RotationMatrixToQuaternion(const T* R, T* quaternion); + +template <typename T, int row_stride, int col_stride> +void RotationMatrixToQuaternion( + const MatrixAdapter<const T, row_stride, col_stride>& R, + T* quaternion); + +// Conversions between 3x3 rotation matrix (in column major order) and +// axis-angle rotation representations. Templated for use with +// autodifferentiation. +template <typename T> +void RotationMatrixToAngleAxis(const T* R, T* angle_axis); + +template <typename T, int row_stride, int col_stride> +void RotationMatrixToAngleAxis( + const MatrixAdapter<const T, row_stride, col_stride>& R, + T* angle_axis); + +template <typename T> +void AngleAxisToRotationMatrix(const T* angle_axis, T* R); + +template <typename T, int row_stride, int col_stride> +void AngleAxisToRotationMatrix( + const T* angle_axis, + const MatrixAdapter<T, row_stride, col_stride>& R); + +// Conversions between 3x3 rotation matrix (in row major order) and +// Euler angle (in degrees) rotation representations. +// +// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} +// axes, respectively. They are applied in that same order, so the +// total rotation R is Rz * Ry * Rx. +template <typename T> +void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); + +template <typename T, int row_stride, int col_stride> +void EulerAnglesToRotationMatrix( + const T* euler, + const MatrixAdapter<T, row_stride, col_stride>& R); + +// Convert a 4-vector to a 3x3 scaled rotation matrix. +// +// The choice of rotation is such that the quaternion [1 0 0 0] goes to an +// identity matrix and for small a, b, c the quaternion [1 a b c] goes to +// the matrix +// +// [ 0 -c b ] +// I + 2 [ c 0 -a ] + higher order terms +// [ -b a 0 ] +// +// which corresponds to a Rodrigues approximation, the last matrix being +// the cross-product matrix of [a b c]. Together with the property that +// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R. +// +// No normalization of the quaternion is performed, i.e. +// R = ||q||^2 * Q, where Q is an orthonormal matrix +// such that det(Q) = 1 and Q*Q' = I +// +// WARNING: The rotation matrix is ROW MAJOR +template <typename T> inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); + +template <typename T, int row_stride, int col_stride> inline +void QuaternionToScaledRotation( + const T q[4], + const MatrixAdapter<T, row_stride, col_stride>& R); + +// Same as above except that the rotation matrix is normalized by the +// Frobenius norm, so that R * R' = I (and det(R) = 1). +// +// WARNING: The rotation matrix is ROW MAJOR +template <typename T> inline +void QuaternionToRotation(const T q[4], T R[3 * 3]); + +template <typename T, int row_stride, int col_stride> inline +void QuaternionToRotation( + const T q[4], + const MatrixAdapter<T, row_stride, col_stride>& R); + +// Rotates a point pt by a quaternion q: +// +// result = R(q) * pt +// +// Assumes the quaternion is unit norm. This assumption allows us to +// write the transform as (something)*pt + pt, as is clear from the +// formula below. If you pass in a quaternion with |q|^2 = 2 then you +// WILL NOT get back 2 times the result you get for a unit quaternion. +template <typename T> inline +void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// With this function you do not need to assume that q has unit norm. +// It does assume that the norm is non-zero. +template <typename T> inline +void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// zw = z * w, where * is the Quaternion product between 4 vectors. +template<typename T> inline +void QuaternionProduct(const T z[4], const T w[4], T zw[4]); + +// xy = x cross y; +template<typename T> inline +void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); + +template<typename T> inline +T DotProduct(const T x[3], const T y[3]); + +// y = R(angle_axis) * x; +template<typename T> inline +void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); + +// --- IMPLEMENTATION + +template<typename T, int row_stride, int col_stride> +struct MatrixAdapter { + T* pointer_; + explicit MatrixAdapter(T* pointer) + : pointer_(pointer) + {} + + T& operator()(int r, int c) const { + return pointer_[r * row_stride + c * col_stride]; + } +}; + +template <typename T> +MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer) { + return MatrixAdapter<T, 1, 3>(pointer); +} + +template <typename T> +MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) { + return MatrixAdapter<T, 3, 1>(pointer); +} + +template<typename T> +inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { + const T& a0 = angle_axis[0]; + const T& a1 = angle_axis[1]; + const T& a2 = angle_axis[2]; + const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2; + + // For points not at the origin, the full conversion is numerically stable. + if (theta_squared > T(0.0)) { + const T theta = sqrt(theta_squared); + const T half_theta = theta * T(0.5); + const T k = sin(half_theta) / theta; + quaternion[0] = cos(half_theta); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } else { + // At the origin, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(0.5); + quaternion[0] = T(1.0); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } +} + +template<typename T> +inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { + const T& q1 = quaternion[1]; + const T& q2 = quaternion[2]; + const T& q3 = quaternion[3]; + const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3; + + // For quaternions representing non-zero rotation, the conversion + // is numerically stable. + if (sin_squared_theta > T(0.0)) { + const T sin_theta = sqrt(sin_squared_theta); + const T& cos_theta = quaternion[0]; + + // If cos_theta is negative, theta is greater than pi/2, which + // means that angle for the angle_axis vector which is 2 * theta + // would be greater than pi. + // + // While this will result in the correct rotation, it does not + // result in a normalized angle-axis vector. + // + // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi, + // which is equivalent saying + // + // theta - pi = atan(sin(theta - pi), cos(theta - pi)) + // = atan(-sin(theta), -cos(theta)) + // + const T two_theta = + T(2.0) * ((cos_theta < 0.0) + ? atan2(-sin_theta, -cos_theta) + : atan2(sin_theta, cos_theta)); + const T k = two_theta / sin_theta; + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } else { + // For zero rotation, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(2.0); + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } +} + +template <typename T> +void RotationMatrixToQuaternion(const T* R, T* angle_axis) { + RotationMatrixToQuaternion(ColumnMajorAdapter3x3(R), angle_axis); +} + +// This algorithm comes from "Quaternion Calculus and Fast Animation", +// Ken Shoemake, 1987 SIGGRAPH course notes +template <typename T, int row_stride, int col_stride> +void RotationMatrixToQuaternion( + const MatrixAdapter<const T, row_stride, col_stride>& R, + T* quaternion) { + const T trace = R(0, 0) + R(1, 1) + R(2, 2); + if (trace >= 0.0) { + T t = sqrt(trace + T(1.0)); + quaternion[0] = T(0.5) * t; + t = T(0.5) / t; + quaternion[1] = (R(2, 1) - R(1, 2)) * t; + quaternion[2] = (R(0, 2) - R(2, 0)) * t; + quaternion[3] = (R(1, 0) - R(0, 1)) * t; + } else { + int i = 0; + if (R(1, 1) > R(0, 0)) { + i = 1; + } + + if (R(2, 2) > R(i, i)) { + i = 2; + } + + const int j = (i + 1) % 3; + const int k = (j + 1) % 3; + T t = sqrt(R(i, i) - R(j, j) - R(k, k) + T(1.0)); + quaternion[i + 1] = T(0.5) * t; + t = T(0.5) / t; + quaternion[0] = (R(k, j) - R(j, k)) * t; + quaternion[j + 1] = (R(j, i) + R(i, j)) * t; + quaternion[k + 1] = (R(k, i) + R(i, k)) * t; + } +} + +// The conversion of a rotation matrix to the angle-axis form is +// numerically problematic when then rotation angle is close to zero +// or to Pi. The following implementation detects when these two cases +// occurs and deals with them by taking code paths that are guaranteed +// to not perform division by a small number. +template <typename T> +inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) { + RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis); +} + +template <typename T, int row_stride, int col_stride> +void RotationMatrixToAngleAxis( + const MatrixAdapter<const T, row_stride, col_stride>& R, + T* angle_axis) { + T quaternion[4]; + RotationMatrixToQuaternion(R, quaternion); + QuaternionToAngleAxis(quaternion, angle_axis); + return; +} + +template <typename T> +inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) { + AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R)); +} + +template <typename T, int row_stride, int col_stride> +void AngleAxisToRotationMatrix( + const T* angle_axis, + const MatrixAdapter<T, row_stride, col_stride>& R) { + static const T kOne = T(1.0); + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > T(std::numeric_limits<double>::epsilon())) { + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + const T theta = sqrt(theta2); + const T wx = angle_axis[0] / theta; + const T wy = angle_axis[1] / theta; + const T wz = angle_axis[2] / theta; + + const T costheta = cos(theta); + const T sintheta = sin(theta); + + R(0, 0) = costheta + wx*wx*(kOne - costheta); + R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta); + R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta); + R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta; + R(1, 1) = costheta + wy*wy*(kOne - costheta); + R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta); + R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta); + R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta); + R(2, 2) = costheta + wz*wz*(kOne - costheta); + } else { + // Near zero, we switch to using the first order Taylor expansion. + R(0, 0) = kOne; + R(1, 0) = angle_axis[2]; + R(2, 0) = -angle_axis[1]; + R(0, 1) = -angle_axis[2]; + R(1, 1) = kOne; + R(2, 1) = angle_axis[0]; + R(0, 2) = angle_axis[1]; + R(1, 2) = -angle_axis[0]; + R(2, 2) = kOne; + } +} + +template <typename T> +inline void EulerAnglesToRotationMatrix(const T* euler, + const int row_stride_parameter, + T* R) { + CHECK_EQ(row_stride_parameter, 3); + EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R)); +} + +template <typename T, int row_stride, int col_stride> +void EulerAnglesToRotationMatrix( + const T* euler, + const MatrixAdapter<T, row_stride, col_stride>& R) { + const double kPi = 3.14159265358979323846; + const T degrees_to_radians(kPi / 180.0); + + const T pitch(euler[0] * degrees_to_radians); + const T roll(euler[1] * degrees_to_radians); + const T yaw(euler[2] * degrees_to_radians); + + const T c1 = cos(yaw); + const T s1 = sin(yaw); + const T c2 = cos(roll); + const T s2 = sin(roll); + const T c3 = cos(pitch); + const T s3 = sin(pitch); + + R(0, 0) = c1*c2; + R(0, 1) = -s1*c3 + c1*s2*s3; + R(0, 2) = s1*s3 + c1*s2*c3; + + R(1, 0) = s1*c2; + R(1, 1) = c1*c3 + s1*s2*s3; + R(1, 2) = -c1*s3 + s1*s2*c3; + + R(2, 0) = -s2; + R(2, 1) = c2*s3; + R(2, 2) = c2*c3; +} + +template <typename T> inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { + QuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); +} + +template <typename T, int row_stride, int col_stride> inline +void QuaternionToScaledRotation( + const T q[4], + const MatrixAdapter<T, row_stride, col_stride>& R) { + // Make convenient names for elements of q. + T a = q[0]; + T b = q[1]; + T c = q[2]; + T d = q[3]; + // This is not to eliminate common sub-expression, but to + // make the lines shorter so that they fit in 80 columns! + T aa = a * a; + T ab = a * b; + T ac = a * c; + T ad = a * d; + T bb = b * b; + T bc = b * c; + T bd = b * d; + T cc = c * c; + T cd = c * d; + T dd = d * d; + + R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT + R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT + R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT +} + +template <typename T> inline +void QuaternionToRotation(const T q[4], T R[3 * 3]) { + QuaternionToRotation(q, RowMajorAdapter3x3(R)); +} + +template <typename T, int row_stride, int col_stride> inline +void QuaternionToRotation(const T q[4], + const MatrixAdapter<T, row_stride, col_stride>& R) { + QuaternionToScaledRotation(q, R); + + T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; + CHECK_NE(normalizer, T(0)); + normalizer = T(1) / normalizer; + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + R(i, j) *= normalizer; + } + } +} + +template <typename T> inline +void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + const T t2 = q[0] * q[1]; + const T t3 = q[0] * q[2]; + const T t4 = q[0] * q[3]; + const T t5 = -q[1] * q[1]; + const T t6 = q[1] * q[2]; + const T t7 = q[1] * q[3]; + const T t8 = -q[2] * q[2]; + const T t9 = q[2] * q[3]; + const T t1 = -q[3] * q[3]; + result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT + result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT + result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT +} + +template <typename T> inline +void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + // 'scale' is 1 / norm(q). + const T scale = T(1) / sqrt(q[0] * q[0] + + q[1] * q[1] + + q[2] * q[2] + + q[3] * q[3]); + + // Make unit-norm version of q. + const T unit[4] = { + scale * q[0], + scale * q[1], + scale * q[2], + scale * q[3], + }; + + UnitQuaternionRotatePoint(unit, pt, result); +} + +template<typename T> inline +void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { + zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; + zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; + zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; + zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; +} + +// xy = x cross y; +template<typename T> inline +void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { + x_cross_y[0] = x[1] * y[2] - x[2] * y[1]; + x_cross_y[1] = x[2] * y[0] - x[0] * y[2]; + x_cross_y[2] = x[0] * y[1] - x[1] * y[0]; +} + +template<typename T> inline +T DotProduct(const T x[3], const T y[3]) { + return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]); +} + +template<typename T> inline +void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > T(std::numeric_limits<double>::epsilon())) { + // Away from zero, use the rodriguez formula + // + // result = pt costheta + + // (w x pt) * sintheta + + // w (w . pt) (1 - costheta) + // + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + // + const T theta = sqrt(theta2); + const T costheta = cos(theta); + const T sintheta = sin(theta); + const T theta_inverse = 1.0 / theta; + + const T w[3] = { angle_axis[0] * theta_inverse, + angle_axis[1] * theta_inverse, + angle_axis[2] * theta_inverse }; + + // Explicitly inlined evaluation of the cross product for + // performance reasons. + const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1], + w[2] * pt[0] - w[0] * pt[2], + w[0] * pt[1] - w[1] * pt[0] }; + const T tmp = + (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta); + + result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp; + result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp; + result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp; + } else { + // Near zero, the first order Taylor approximation of the rotation + // matrix R corresponding to a vector w and angle w is + // + // R = I + hat(w) * sin(theta) + // + // But sintheta ~ theta and theta * w = angle_axis, which gives us + // + // R = I + hat(w) + // + // and actually performing multiplication with the point pt, gives us + // R * pt = pt + w x pt. + // + // Switching to the Taylor expansion near zero provides meaningful + // derivatives when evaluated using Jets. + // + // Explicitly inlined evaluation of the cross product for + // performance reasons. + const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1], + angle_axis[2] * pt[0] - angle_axis[0] * pt[2], + angle_axis[0] * pt[1] - angle_axis[1] * pt[0] }; + + result[0] = pt[0] + w_cross_pt[0]; + result[1] = pt[1] + w_cross_pt[1]; + result[2] = pt[2] + w_cross_pt[2]; + } +} + +} // namespace ceres + +#endif // CERES_PUBLIC_ROTATION_H_ diff --git a/extern/ceres/include/ceres/sized_cost_function.h b/extern/ceres/include/ceres/sized_cost_function.h new file mode 100644 index 00000000000..b10421e81be --- /dev/null +++ b/extern/ceres/include/ceres/sized_cost_function.h @@ -0,0 +1,96 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A convenience class for cost functions which are statically sized. +// Compared to the dynamically-sized base class, this reduces boilerplate. +// +// The kNumResiduals template parameter can be a constant such as 2 or 5, or it +// can be ceres::DYNAMIC. If kNumResiduals is ceres::DYNAMIC, then subclasses +// are responsible for calling set_num_residuals() at runtime. + +#ifndef CERES_PUBLIC_SIZED_COST_FUNCTION_H_ +#define CERES_PUBLIC_SIZED_COST_FUNCTION_H_ + +#include "ceres/types.h" +#include "ceres/cost_function.h" +#include "glog/logging.h" + +namespace ceres { + +template<int kNumResiduals, + int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, + int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> +class SizedCostFunction : public CostFunction { + public: + SizedCostFunction() { + CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC) + << "Cost functions must have at least one residual block."; + + // This block breaks the 80 column rule to keep it somewhat readable. + CHECK((!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) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT + << "Zero block cannot precede a non-zero block. Block sizes are " + << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " + << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " + << N8 << ", " << N9; + + set_num_residuals(kNumResiduals); + +#define CERES_ADD_PARAMETER_BLOCK(N) \ + if (N) mutable_parameter_block_sizes()->push_back(N); + CERES_ADD_PARAMETER_BLOCK(N0); + CERES_ADD_PARAMETER_BLOCK(N1); + CERES_ADD_PARAMETER_BLOCK(N2); + CERES_ADD_PARAMETER_BLOCK(N3); + CERES_ADD_PARAMETER_BLOCK(N4); + CERES_ADD_PARAMETER_BLOCK(N5); + CERES_ADD_PARAMETER_BLOCK(N6); + CERES_ADD_PARAMETER_BLOCK(N7); + CERES_ADD_PARAMETER_BLOCK(N8); + CERES_ADD_PARAMETER_BLOCK(N9); +#undef CERES_ADD_PARAMETER_BLOCK + } + + virtual ~SizedCostFunction() { } + + // Subclasses must implement Evaluate(). +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_SIZED_COST_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/solver.h b/extern/ceres/include/ceres/solver.h new file mode 100644 index 00000000000..318cf48cb83 --- /dev/null +++ b/extern/ceres/include/ceres/solver.h @@ -0,0 +1,1028 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_SOLVER_H_ +#define CERES_PUBLIC_SOLVER_H_ + +#include <cmath> +#include <string> +#include <vector> +#include "ceres/crs_matrix.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/iteration_callback.h" +#include "ceres/ordered_groups.h" +#include "ceres/types.h" +#include "ceres/internal/disable_warnings.h" + +namespace ceres { + +class Problem; + +// Interface for non-linear least squares solvers. +class CERES_EXPORT Solver { + public: + virtual ~Solver(); + + // The options structure contains, not surprisingly, options that control how + // the solver operates. The defaults should be suitable for a wide range of + // problems; however, better performance is often obtainable with tweaking. + // + // The constants are defined inside types.h + struct CERES_EXPORT Options { + // Default constructor that sets up a generic sparse problem. + Options() { + minimizer_type = TRUST_REGION; + line_search_direction_type = LBFGS; + 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; + max_consecutive_nonmonotonic_steps = 5; + max_num_iterations = 50; + max_solver_time_in_seconds = 1e9; + num_threads = 1; + initial_trust_region_radius = 1e4; + max_trust_region_radius = 1e16; + min_trust_region_radius = 1e-32; + min_relative_decrease = 1e-3; + min_lm_diagonal = 1e-6; + max_lm_diagonal = 1e32; + max_num_consecutive_invalid_steps = 5; + function_tolerance = 1e-6; + gradient_tolerance = 1e-10; + parameter_tolerance = 1e-8; + +#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) && !defined(CERES_ENABLE_LGPL_CODE) // NOLINT + linear_solver_type = DENSE_QR; +#else + linear_solver_type = SPARSE_NORMAL_CHOLESKY; +#endif + + preconditioner_type = JACOBI; + visibility_clustering_type = CANONICAL_VIEWS; + dense_linear_algebra_library_type = EIGEN; + + // Choose a default sparse linear algebra library in the order: + // + // SUITE_SPARSE > CX_SPARSE > EIGEN_SPARSE > NO_SPARSE + sparse_linear_algebra_library_type = NO_SPARSE; +#if !defined(CERES_NO_SUITESPARSE) + sparse_linear_algebra_library_type = SUITE_SPARSE; +#else + #if !defined(CERES_NO_CXSPARSE) + sparse_linear_algebra_library_type = CX_SPARSE; + #else + #if defined(CERES_USE_EIGEN_SPARSE) + sparse_linear_algebra_library_type = EIGEN_SPARSE; + #endif + #endif +#endif + + num_linear_solver_threads = 1; + use_explicit_schur_complement = false; + use_postordering = false; + dynamic_sparsity = false; + min_linear_solver_iterations = 0; + max_linear_solver_iterations = 500; + eta = 1e-1; + jacobi_scaling = true; + use_inner_iterations = false; + inner_iteration_tolerance = 1e-3; + logging_type = PER_MINIMIZER_ITERATION; + minimizer_progress_to_stdout = false; + 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; + update_state_every_iteration = false; + } + + // Returns true if the options struct has a valid + // configuration. Returns false otherwise, and fills in *error + // with a message describing the problem. + bool IsValid(std::string* error) const; + + // Minimizer options ---------------------------------------- + + // Ceres supports the two major families of optimization strategies - + // Trust Region and Line Search. + // + // 1. The line search approach first finds a descent direction + // along which the objective function will be reduced and then + // computes a step size that decides how far should move along + // that direction. The descent direction can be computed by + // various methods, such as gradient descent, Newton's method and + // Quasi-Newton method. The step size can be determined either + // exactly or inexactly. + // + // 2. The trust region approach approximates the objective + // function using using a model function (often a quadratic) over + // a subset of the search space known as the trust region. If the + // model function succeeds in minimizing the true objective + // function the trust region is expanded; conversely, otherwise it + // is contracted and the model optimization problem is solved + // again. + // + // Trust region methods are in some sense dual to line search methods: + // trust region methods first choose a step size (the size of the + // trust region) and then a step direction while line search methods + // first choose a step direction and then a step size. + MinimizerType minimizer_type; + + LineSearchDirectionType line_search_direction_type; + LineSearchType line_search_type; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; + + // The LBFGS hessian approximation is a low rank approximation to + // the inverse of the Hessian matrix. The rank of the + // approximation determines (linearly) the space and time + // complexity of using the approximation. Higher the rank, the + // better is the quality of the approximation. The increase in + // quality is however is bounded for a number of reasons. + // + // 1. The method only uses secant information and not actual + // derivatives. + // + // 2. The Hessian approximation is constrained to be positive + // definite. + // + // So increasing this rank to a large number will cost time and + // space complexity without the corresponding increase in solution + // quality. There are no hard and fast rules for choosing the + // maximum rank. The best choice usually requires some problem + // specific experimentation. + // + // For more theoretical and implementation details of the LBFGS + // method, please see: + // + // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with + // 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. + DoglegType dogleg_type; + + // The classical trust region methods are descent methods, in that + // they only accept a point if it strictly reduces the value of + // the objective function. + // + // Relaxing this requirement allows the algorithm to be more + // efficient in the long term at the cost of some local increase + // in the value of the objective function. + // + // This is because allowing for non-decreasing objective function + // values in a princpled manner allows the algorithm to "jump over + // boulders" as the method is not restricted to move into narrow + // valleys while preserving its convergence properties. + // + // Setting use_nonmonotonic_steps to true enables the + // non-monotonic trust region algorithm as described by Conn, + // Gould & Toint in "Trust Region Methods", Section 10.1. + // + // The parameter max_consecutive_nonmonotonic_steps controls the + // window size used by the step selection algorithm to accept + // non-monotonic steps. + // + // Even though the value of the objective function may be larger + // than the minimum value encountered over the course of the + // optimization, the final parameters returned to the user are the + // ones corresponding to the minimum cost over all iterations. + bool use_nonmonotonic_steps; + int max_consecutive_nonmonotonic_steps; + + // Maximum number of iterations for the minimizer to run for. + int max_num_iterations; + + // Maximum time for which the minimizer should run for. + double max_solver_time_in_seconds; + + // Number of threads used by Ceres for evaluating the cost and + // jacobians. + int num_threads; + + // Trust region minimizer settings. + double initial_trust_region_radius; + double max_trust_region_radius; + + // Minimizer terminates when the trust region radius becomes + // smaller than this value. + double min_trust_region_radius; + + // Lower bound for the relative decrease before a step is + // accepted. + double min_relative_decrease; + + // For the Levenberg-Marquadt algorithm, the scaled diagonal of + // the normal equations J'J is used to control the size of the + // trust region. Extremely small and large values along the + // diagonal can make this regularization scheme + // fail. 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 min_lm_diagonal; + double max_lm_diagonal; + + // Sometimes due to numerical conditioning problems or linear + // solver flakiness, the trust region strategy may return a + // numerically invalid step that can be fixed by reducing the + // trust region size. So the TrustRegionMinimizer allows for a few + // successive invalid steps before it declares NUMERICAL_FAILURE. + int max_num_consecutive_invalid_steps; + + // Minimizer terminates when + // + // (new_cost - old_cost) < function_tolerance * old_cost; + // + double function_tolerance; + + // Minimizer terminates when + // + // max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance + // + // This value should typically be 1e-4 * function_tolerance. + double gradient_tolerance; + + // Minimizer terminates when + // + // |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance) + // + double parameter_tolerance; + + // Linear least squares solver options ------------------------------------- + + LinearSolverType linear_solver_type; + + // Type of preconditioner to use with the iterative linear solvers. + PreconditionerType preconditioner_type; + + // Type of clustering algorithm to use for visibility based + // preconditioning. This option is used only when the + // preconditioner_type is CLUSTER_JACOBI or CLUSTER_TRIDIAGONAL. + VisibilityClusteringType visibility_clustering_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_type; + + // Number of threads used by Ceres to solve the Newton + // step. Currently only the SPARSE_SCHUR solver is capable of + // using this setting. + int num_linear_solver_threads; + + // The order in which variables are eliminated in a linear solver + // can have a significant of impact on the efficiency and accuracy + // of the method. e.g., when doing sparse Cholesky factorization, + // there are matrices for which a good ordering will give a + // Cholesky factor with O(n) storage, where as a bad ordering will + // result in an completely dense factor. + // + // Ceres allows the user to provide varying amounts of hints to + // the solver about the variable elimination ordering to use. This + // can range from no hints, where the solver is free to decide the + // best possible ordering based on the user's choices like the + // linear solver being used, to an exact order in which the + // variables should be eliminated, and a variety of possibilities + // in between. + // + // Instances of the ParameterBlockOrdering class are used to + // communicate this information to Ceres. + // + // Formally an ordering is an ordered partitioning of the + // parameter blocks, i.e, each parameter block belongs to exactly + // one group, and each group has a unique non-negative integer + // associated with it, that determines its order in the set of + // groups. + // + // Given such an ordering, Ceres ensures that the parameter blocks in + // the lowest numbered group are eliminated first, and then the + // parmeter blocks in the next lowest numbered group and so on. Within + // each group, Ceres is free to order the parameter blocks as it + // chooses. + // + // If NULL, then all parameter blocks are assumed to be in the + // same group and the solver is free to decide the best + // ordering. + // + // e.g. Consider the linear system + // + // x + y = 3 + // 2x + 3y = 7 + // + // There are two ways in which it can be solved. First eliminating x + // from the two equations, solving for y and then back substituting + // for x, or first eliminating y, solving for x and back substituting + // for y. The user can construct three orderings here. + // + // {0: x}, {1: y} - eliminate x first. + // {0: y}, {1: x} - eliminate y first. + // {0: x, y} - Solver gets to decide the elimination order. + // + // Thus, to have Ceres determine the ordering automatically using + // heuristics, put all the variables in group 0 and to control the + // ordering for every variable, create groups 0..N-1, one per + // variable, in the desired order. + // + // Bundle Adjustment + // ----------------- + // + // A particular case of interest is bundle adjustment, where the user + // has two options. The default is to not specify an ordering at all, + // the solver will see that the user wants to use a Schur type solver + // and figure out the right elimination ordering. + // + // But if the user already knows what parameter blocks are points and + // what are cameras, they can save preprocessing time by partitioning + // the parameter blocks into two groups, one for the points and one + // for the cameras, where the group containing the points has an id + // smaller than the group containing cameras. + shared_ptr<ParameterBlockOrdering> linear_solver_ordering; + + // Use an explicitly computed Schur complement matrix with + // ITERATIVE_SCHUR. + // + // By default this option is disabled and ITERATIVE_SCHUR + // evaluates evaluates matrix-vector products between the Schur + // complement and a vector implicitly by exploiting the algebraic + // expression for the Schur complement. + // + // The cost of this evaluation scales with the number of non-zeros + // in the Jacobian. + // + // For small to medium sized problems there is a sweet spot where + // computing the Schur complement is cheap enough that it is much + // more efficient to explicitly compute it and use it for evaluating + // the matrix-vector products. + // + // Enabling this option tells ITERATIVE_SCHUR to use an explicitly + // computed Schur complement. + // + // NOTE: This option can only be used with the SCHUR_JACOBI + // preconditioner. + bool use_explicit_schur_complement; + + // 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, thus 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 use_postordering to true enables this tradeoff. + bool use_postordering; + + // Some non-linear least squares problems are symbolically dense but + // numerically sparse. i.e. at any given state only a small number + // of jacobian entries are non-zero, but the position and number of + // non-zeros is different depending on the state. For these problems + // it can be useful to factorize the sparse jacobian at each solver + // iteration instead of including all of the zero entries in a single + // general factorization. + // + // If your problem does not have this property (or you do not know), + // then it is probably best to keep this false, otherwise it will + // likely lead to worse performance. + + // This settings affects the SPARSE_NORMAL_CHOLESKY solver. + bool dynamic_sparsity; + + // Some non-linear least squares problems have additional + // structure in the way the parameter blocks interact that it is + // beneficial to modify the way the trust region step is computed. + // + // e.g., consider the following regression problem + // + // y = a_1 exp(b_1 x) + a_2 exp(b_3 x^2 + c_1) + // + // Given a set of pairs{(x_i, y_i)}, the user wishes to estimate + // a_1, a_2, b_1, b_2, and c_1. + // + // Notice here that the expression on the left is linear in a_1 + // and a_2, and given any value for b_1, b_2 and c_1, it is + // possible to use linear regression to estimate the optimal + // values of a_1 and a_2. Indeed, its possible to analytically + // eliminate the variables a_1 and a_2 from the problem all + // together. Problems like these are known as separable least + // squares problem and the most famous algorithm for solving them + // is the Variable Projection algorithm invented by Golub & + // Pereyra. + // + // Similar structure can be found in the matrix factorization with + // missing data problem. There the corresponding algorithm is + // known as Wiberg's algorithm. + // + // Ruhe & Wedin (Algorithms for Separable Nonlinear Least Squares + // Problems, SIAM Reviews, 22(3), 1980) present an analyis of + // various algorithms for solving separable non-linear least + // squares problems and refer to "Variable Projection" as + // Algorithm I in their paper. + // + // Implementing Variable Projection is tedious and expensive, and + // they present a simpler algorithm, which they refer to as + // Algorithm II, where once the Newton/Trust Region step has been + // computed for the whole problem (a_1, a_2, b_1, b_2, c_1) and + // additional optimization step is performed to estimate a_1 and + // a_2 exactly. + // + // This idea can be generalized to cases where the residual is not + // linear in a_1 and a_2, i.e., Solve for the trust region step + // for the full problem, and then use it as the starting point to + // further optimize just a_1 and a_2. For the linear case, this + // amounts to doing a single linear least squares solve. For + // non-linear problems, any method for solving the a_1 and a_2 + // optimization problems will do. The only constraint on a_1 and + // a_2 is that they do not co-occur in any residual block. + // + // This idea can be further generalized, by not just optimizing + // (a_1, a_2), but decomposing the graph corresponding to the + // Hessian matrix's sparsity structure in a collection of + // non-overlapping independent sets and optimizing each of them. + // + // Setting "use_inner_iterations" to true enables the use of this + // non-linear generalization of Ruhe & Wedin's Algorithm II. This + // version of Ceres has a higher iteration complexity, but also + // displays better convergence behaviour per iteration. Setting + // Solver::Options::num_threads to the maximum number possible is + // highly recommended. + bool use_inner_iterations; + + // If inner_iterations is true, then the user has two choices. + // + // 1. Let the solver heuristically decide which parameter blocks + // to optimize in each inner iteration. To do this leave + // Solver::Options::inner_iteration_ordering untouched. + // + // 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. Not + // all parameter blocks need to be present in the ordering. + shared_ptr<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 min_linear_solver_iterations; + + // Maximum number of iterations for which the linear solver should + // run. If the solver does not converge in less than + // 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 + // Newton step is computed. + // + // This constant is passed to ConjugateGradientsSolver which uses + // it to terminate the iterations when + // + // (Q_i - Q_{i-1})/Q_i < eta/i + double eta; + + // Normalize the jacobian using Jacobi scaling before calling + // the linear least squares solver. + bool jacobi_scaling; + + // Logging options --------------------------------------------------------- + + LoggingType logging_type; + + // By default the Minimizer progress is logged to VLOG(1), which + // is sent to STDERR depending on the vlog level. If this flag is + // set to true, and logging_type is not SILENT, the logging output + // is sent to STDOUT. + bool minimizer_progress_to_stdout; + + // 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. + std::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. + std::string trust_region_problem_dump_directory; + DumpFormatType trust_region_problem_dump_format_type; + + // Finite differences options ---------------------------------------------- + + // Check all jacobians computed by each residual block with finite + // differences. This is expensive since it involves computing the + // derivative by normal means (e.g. user specified, autodiff, + // etc), then also computing it using finite differences. The + // results are compared, and if they differ substantially, details + // are printed to the log. + bool check_gradients; + + // Relative precision to check for in the gradient checker. If the + // relative difference between an element in a jacobian exceeds + // this number, then the jacobian for that cost term is dumped. + double gradient_check_relative_precision; + + // Relative shift used for taking numeric derivatives. For finite + // differencing, each dimension is evaluated at slightly shifted + // values; for the case of central difference, this is what gets + // evaluated: + // + // delta = numeric_derivative_relative_step_size; + // f_initial = f(x) + // f_forward = f((1 + delta) * x) + // f_backward = f((1 - delta) * x) + // + // The finite differencing is done along each dimension. The + // reason to use a relative (rather than absolute) step size is + // that this way, numeric differentation works for functions where + // the arguments are typically large (e.g. 1e9) and when the + // values are small (e.g. 1e-5). It is possible to construct + // "torture cases" which break this finite difference heuristic, + // but they do not come up often in practice. + // + // TODO(keir): Pick a smarter number than the default above! In + // theory a good choice is sqrt(eps) * x, which for doubles means + // about 1e-8 * x. However, I have found this number too + // optimistic. This number should be exposed for users to change. + double numeric_derivative_relative_step_size; + + // If true, the user's parameter blocks are updated at the end of + // every Minimizer iteration, otherwise they are updated when the + // Minimizer terminates. This is useful if, for example, the user + // wishes to visualize the state of the optimization every + // iteration. + bool update_state_every_iteration; + + // Callbacks that are executed at the end of each iteration of the + // Minimizer. An iteration may terminate midway, either due to + // numerical failures or because one of the convergence tests has + // been satisfied. In this case none of the callbacks are + // executed. + + // Callbacks are executed in the order that they are specified in + // this vector. By default, parameter blocks are updated only at + // the end of the optimization, i.e when the Minimizer + // terminates. This behaviour is controlled by + // update_state_every_variable. If the user wishes to have access + // to the update parameter blocks when his/her callbacks are + // executed, then set update_state_every_iteration to true. + // + // The solver does NOT take ownership of these pointers. + std::vector<IterationCallback*> callbacks; + }; + + struct CERES_EXPORT Summary { + Summary(); + + // A brief one line description of the state of the solver after + // termination. + std::string BriefReport() const; + + // A full multiline description of the state of the solver after + // termination. + std::string FullReport() const; + + bool IsSolutionUsable() const; + + // Minimizer summary ------------------------------------------------- + MinimizerType minimizer_type; + + TerminationType termination_type; + + // Reason why the solver terminated. + std::string message; + + // Cost of the problem (value of the objective function) before + // the optimization. + double initial_cost; + + // Cost of the problem (value of the objective function) after the + // optimization. + double final_cost; + + // The part of the total cost that comes from residual blocks that + // were held fixed by the preprocessor because all the parameter + // blocks that they depend on were fixed. + double fixed_cost; + + // IterationSummary for each minimizer iteration in order. + std::vector<IterationSummary> iterations; + + // Number of minimizer iterations in which the step was + // accepted. Unless use_non_monotonic_steps is true this is also + // the number of steps in which the objective function value/cost + // went down. + int num_successful_steps; + + // Number of minimizer iterations in which the step was rejected + // either because it did not reduce the cost enough or the step + // was not numerically valid. + int num_unsuccessful_steps; + + // Number of times inner iterations were performed. + 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 + // include error checks, memory allocations, and reorderings. This + // time is accounted for as preprocessing time. + double preprocessor_time_in_seconds; + + // Time spent in the TrustRegionMinimizer. + double minimizer_time_in_seconds; + + // After the Minimizer is finished, some time is spent in + // re-evaluating residuals etc. This time is accounted for in the + // postprocessor time. + double postprocessor_time_in_seconds; + + // Some total of all time spent inside Ceres when Solve is called. + double total_time_in_seconds; + + // Time (in seconds) spent in the linear solver computing the + // trust region step. + double linear_solver_time_in_seconds; + + // Time (in seconds) spent evaluating the residual vector. + double residual_evaluation_time_in_seconds; + + // Time (in seconds) spent evaluating the jacobian matrix. + double jacobian_evaluation_time_in_seconds; + + // Time (in seconds) spent doing inner iterations. + double inner_iteration_time_in_seconds; + + // Cumulative timing information for line searches performed as part of the + // solve. Note that in addition to the case when the Line Search minimizer + // is used, the Trust Region minimizer also uses a line search when + // solving a constrained problem. + + // Time (in seconds) spent evaluating the univariate cost function as part + // of a line search. + double line_search_cost_evaluation_time_in_seconds; + + // Time (in seconds) spent evaluating the gradient of the univariate cost + // function as part of a line search. + double line_search_gradient_evaluation_time_in_seconds; + + // Time (in seconds) spent minimizing the interpolating polynomial + // to compute the next candidate step size as part of a line search. + double line_search_polynomial_minimization_time_in_seconds; + + // Total time (in seconds) spent performing line searches. + double line_search_total_time_in_seconds; + + // Number of parameter blocks in the problem. + int num_parameter_blocks; + + // Number of parameters in the probem. + int num_parameters; + + // Dimension of the tangent space of the problem (or the number of + // columns in the Jacobian for the problem). This is different + // from num_parameters if a parameter block is associated with a + // LocalParameterization + int num_effective_parameters; + + // Number of residual blocks in the problem. + int num_residual_blocks; + + // Number of residuals in the problem. + int num_residuals; + + // Number of parameter blocks in the problem after the inactive + // and constant parameter blocks have been removed. A parameter + // block is inactive if no residual block refers to it. + int num_parameter_blocks_reduced; + + // Number of parameters in the reduced problem. + int num_parameters_reduced; + + // Dimension of the tangent space of the reduced problem (or the + // number of columns in the Jacobian for the reduced + // problem). This is different from num_parameters_reduced if a + // parameter block in the reduced problem is associated with a + // LocalParameterization. + int num_effective_parameters_reduced; + + // Number of residual blocks in the reduced problem. + int num_residual_blocks_reduced; + + // Number of residuals in the reduced problem. + int num_residuals_reduced; + + // Is the reduced problem bounds constrained. + bool is_constrained; + + // Number of threads specified by the user for Jacobian and + // residual evaluation. + int num_threads_given; + + // Number of threads actually used by the solver for Jacobian and + // residual evaluation. This number is not equal to + // num_threads_given if OpenMP is not available. + int num_threads_used; + + // Number of threads specified by the user for solving the trust + // region problem. + int num_linear_solver_threads_given; + + // Number of threads actually used by the solver for solving the + // trust region problem. This number is not equal to + // num_threads_given if OpenMP is not available. + int num_linear_solver_threads_used; + + // Type of the linear solver requested by the user. + LinearSolverType linear_solver_type_given; + + // Type of the linear solver actually used. This may be different + // from linear_solver_type_given if Ceres determines that the + // problem structure is not compatible with the linear solver + // requested or if the linear solver requested by the user is not + // available, e.g. The user requested SPARSE_NORMAL_CHOLESKY but + // no sparse linear algebra library was available. + LinearSolverType linear_solver_type_used; + + // Size of the elimination groups given by the user as hints to + // the linear solver. + std::vector<int> linear_solver_ordering_given; + + // Size of the parameter groups used by the solver when ordering + // the columns of the Jacobian. This maybe different from + // linear_solver_ordering_given if the user left + // linear_solver_ordering_given blank and asked for an automatic + // ordering, or if the problem contains some constant or inactive + // parameter blocks. + std::vector<int> linear_solver_ordering_used; + + // True if the user asked for inner iterations to be used as part + // of the optimization. + bool inner_iterations_given; + + // True if the user asked for inner iterations to be used as part + // of the optimization and the problem structure was such that + // they were actually performed. e.g., in a problem with just one + // parameter block, inner iterations are not performed. + bool inner_iterations_used; + + // Size of the parameter groups given by the user for performing + // inner iterations. + std::vector<int> inner_iteration_ordering_given; + + // Size of the parameter groups given used by the solver for + // performing inner iterations. This maybe different from + // inner_iteration_ordering_given if the user left + // inner_iteration_ordering_given blank and asked for an automatic + // ordering, or if the problem contains some constant or inactive + // parameter blocks. + std::vector<int> inner_iteration_ordering_used; + + // Type of the preconditioner requested by the user. + PreconditionerType preconditioner_type_given; + + // Type of the preconditioner actually used. This may be different + // from linear_solver_type_given if Ceres determines that the + // problem structure is not compatible with the linear solver + // requested or if the linear solver requested by the user is not + // available. + PreconditionerType preconditioner_type_used; + + // Type of clustering algorithm used for visibility based + // preconditioning. Only meaningful when the preconditioner_type + // is CLUSTER_JACOBI or CLUSTER_TRIDIAGONAL. + VisibilityClusteringType visibility_clustering_type; + + // Type of trust region strategy. + TrustRegionStrategyType trust_region_strategy_type; + + // Type of dogleg strategy used for solving the trust region + // problem. + DoglegType dogleg_type; + + // Type of the dense linear algebra library used. + DenseLinearAlgebraLibraryType dense_linear_algebra_library_type; + + // Type of the sparse linear algebra library used. + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; + + // Type of line search direction used. + LineSearchDirectionType line_search_direction_type; + + // Type of the line search algorithm used. + LineSearchType line_search_type; + + // When performing line search, the degree of the polynomial used + // to approximate the objective function. + LineSearchInterpolationType line_search_interpolation_type; + + // If the line search direction is NONLINEAR_CONJUGATE_GRADIENT, + // then this indicates the particular variant of non-linear + // conjugate gradient used. + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; + + // If the type of the line search direction is LBFGS, then this + // indicates the rank of the Hessian approximation. + int max_lbfgs_rank; + }; + + // Once a least squares problem has been built, this function takes + // the problem and optimizes it based on the values of the options + // parameters. Upon return, a detailed summary of the work performed + // by the preprocessor, the non-linear minmizer and the linear + // solver are reported in the summary object. + virtual void Solve(const Options& options, + Problem* problem, + Solver::Summary* summary); +}; + +// Helper function which avoids going through the interface. +CERES_EXPORT void Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary); + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_SOLVER_H_ diff --git a/extern/ceres/include/ceres/types.h b/extern/ceres/include/ceres/types.h new file mode 100644 index 00000000000..2ea41803629 --- /dev/null +++ b/extern/ceres/include/ceres/types.h @@ -0,0 +1,508 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Enums and other top level class definitions. +// +// Note: internal/types.cc defines stringification routines for some +// of these enums. Please update those routines if you extend or +// remove enums from here. + +#ifndef CERES_PUBLIC_TYPES_H_ +#define CERES_PUBLIC_TYPES_H_ + +#include <string> + +#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" + +namespace ceres { + +// Basic integer types. These typedefs are in the Ceres namespace to avoid +// conflicts with other packages having similar typedefs. +typedef int int32; + +// Argument type used in interfaces that can optionally take ownership +// of a passed in argument. If TAKE_OWNERSHIP is passed, the called +// object takes ownership of the pointer argument, and will call +// delete on it upon completion. +enum Ownership { + DO_NOT_TAKE_OWNERSHIP, + TAKE_OWNERSHIP +}; + +// TODO(keir): Considerably expand the explanations of each solver type. +enum LinearSolverType { + // These solvers are for general rectangular systems formed from the + // normal equations A'A x = A'b. They are direct solvers and do not + // assume any special problem structure. + + // Solve the normal equations using a dense Cholesky solver; based + // on Eigen. + DENSE_NORMAL_CHOLESKY, + + // Solve the normal equations using a dense QR solver; based on + // Eigen. + DENSE_QR, + + // Solve the normal equations using a sparse cholesky solver; requires + // SuiteSparse or CXSparse. + SPARSE_NORMAL_CHOLESKY, + + // Specialized solvers, specific to problems with a generalized + // bi-partitite structure. + + // Solves the reduced linear system using a dense Cholesky solver; + // based on Eigen. + DENSE_SCHUR, + + // Solves the reduced linear system using a sparse Cholesky solver; + // based on CHOLMOD. + SPARSE_SCHUR, + + // Solves the reduced linear system using Conjugate Gradients, based + // on a new Ceres implementation. Suitable for large scale + // problems. + ITERATIVE_SCHUR, + + // Conjugate gradients on the normal equations. + CGNR +}; + +enum PreconditionerType { + // Trivial preconditioner - the identity matrix. + IDENTITY, + + // Block diagonal of the Gauss-Newton Hessian. + JACOBI, + + // Note: The following three preconditioners can only be used with + // the ITERATIVE_SCHUR solver. They are well suited for Structure + // from Motion problems. + + // Block diagonal of the Schur complement. This preconditioner may + // only be used with the ITERATIVE_SCHUR solver. + SCHUR_JACOBI, + + // Visibility clustering based preconditioners. + // + // The following two preconditioners use the visibility structure of + // the scene to determine the sparsity structure of the + // preconditioner. This is done using a clustering algorithm. The + // available visibility clustering algorithms are described below. + // + // Note: Requires SuiteSparse. + CLUSTER_JACOBI, + CLUSTER_TRIDIAGONAL +}; + +enum VisibilityClusteringType { + // Canonical views algorithm as described in + // + // "Scene Summarization for Online Image Collections", Ian Simon, Noah + // Snavely, Steven M. Seitz, ICCV 2007. + // + // This clustering algorithm can be quite slow, but gives high + // quality clusters. The original visibility based clustering paper + // used this algorithm. + CANONICAL_VIEWS, + + // The classic single linkage algorithm. It is extremely fast as + // compared to CANONICAL_VIEWS, but can give slightly poorer + // results. For problems with large number of cameras though, this + // is generally a pretty good option. + // + // If you are using SCHUR_JACOBI preconditioner and have SuiteSparse + // available, CLUSTER_JACOBI and CLUSTER_TRIDIAGONAL in combination + // with the SINGLE_LINKAGE algorithm will generally give better + // results. + SINGLE_LINKAGE +}; + +enum SparseLinearAlgebraLibraryType { + // High performance sparse Cholesky factorization and approximate + // minimum degree ordering. + SUITE_SPARSE, + + // A lightweight replacment for SuiteSparse, which does not require + // a LAPACK/BLAS implementation. Consequently, its performance is + // also a bit lower than SuiteSparse. + CX_SPARSE, + + // Eigen's sparse linear algebra routines. In particular Ceres uses + // the Simplicial LDLT routines. + EIGEN_SPARSE, + + // No sparse linear solver should be used. This does not necessarily + // imply that Ceres was built without any sparse library, although that + // is the likely use case, merely that one should not be used. + NO_SPARSE +}; + +enum DenseLinearAlgebraLibraryType { + EIGEN, + LAPACK +}; + +// Logging options +// The options get progressively noisier. +enum LoggingType { + SILENT, + PER_MINIMIZER_ITERATION +}; + +enum MinimizerType { + LINE_SEARCH, + TRUST_REGION +}; + +enum LineSearchDirectionType { + // Negative of the gradient. + STEEPEST_DESCENT, + + // A generalization of the Conjugate Gradient method to non-linear + // functions. The generalization can be performed in a number of + // different ways, resulting in a variety of search directions. The + // precise choice of the non-linear conjugate gradient algorithm + // used is determined by NonlinerConjuateGradientType. + NONLINEAR_CONJUGATE_GRADIENT, + + // 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. + // + // 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. + // + // Byrd, R. H.; Nocedal, J.; Schnabel, R. B. (1994). + // "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 +// method of Conjugate Gradients for linear systems. The +// generalization can be carried out in a number of different ways +// leading to number of different rules for computing the search +// direction. Ceres provides a number of different variants. For more +// details see Numerical Optimization by Nocedal & Wright. +enum NonlinearConjugateGradientType { + FLETCHER_REEVES, + POLAK_RIBIERE, + HESTENES_STIEFEL, +}; + +enum LineSearchType { + // Backtracking line search with polynomial interpolation or + // bisection. + ARMIJO, + WOLFE, +}; + +// Ceres supports different strategies for computing the trust region +// step. +enum TrustRegionStrategyType { + // The default trust region strategy is to use the step computation + // used in the Levenberg-Marquardt algorithm. For more details see + // levenberg_marquardt_strategy.h + LEVENBERG_MARQUARDT, + + // Powell's dogleg algorithm interpolates between the Cauchy point + // and the Gauss-Newton step. It is particularly useful if the + // LEVENBERG_MARQUARDT algorithm is making a large number of + // unsuccessful steps. For more details see dogleg_strategy.h. + // + // NOTES: + // + // 1. This strategy has not been experimented with or tested as + // extensively as LEVENBERG_MARQUARDT, and therefore it should be + // considered EXPERIMENTAL for now. + // + // 2. For now this strategy should only be used with exact + // factorization based linear solvers, i.e., SPARSE_SCHUR, + // DENSE_SCHUR, DENSE_QR and SPARSE_NORMAL_CHOLESKY. + DOGLEG +}; + +// Ceres supports two different dogleg strategies. +// The "traditional" dogleg method by Powell and the +// "subspace" method described in +// R. H. Byrd, R. B. Schnabel, and G. A. Shultz, +// "Approximate solution of the trust region problem by minimization +// over two-dimensional subspaces", Mathematical Programming, +// 40 (1988), pp. 247--263 +enum DoglegType { + // The traditional approach constructs a dogleg path + // consisting of two line segments and finds the furthest + // point on that path that is still inside the trust region. + TRADITIONAL_DOGLEG, + + // The subspace approach finds the exact minimum of the model + // constrained to the subspace spanned by the dogleg path. + SUBSPACE_DOGLEG +}; + +enum TerminationType { + // Minimizer terminated because one of the convergence criterion set + // by the user was satisfied. + // + // 1. (new_cost - old_cost) < function_tolerance * old_cost; + // 2. max_i |gradient_i| < gradient_tolerance + // 3. |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance) + // + // The user's parameter blocks will be updated with the solution. + CONVERGENCE, + + // The solver ran for maximum number of iterations or maximum amount + // of time specified by the user, but none of the convergence + // criterion specified by the user were met. The user's parameter + // blocks will be updated with the solution found so far. + NO_CONVERGENCE, + + // The minimizer terminated because of an error. The user's + // parameter blocks will not be updated. + FAILURE, + + // Using an IterationCallback object, user code can control the + // minimizer. The following enums indicate that the user code was + // responsible for termination. + // + // Minimizer terminated successfully because a user + // IterationCallback returned SOLVER_TERMINATE_SUCCESSFULLY. + // + // The user's parameter blocks will be updated with the solution. + USER_SUCCESS, + + // Minimizer terminated because because a user IterationCallback + // returned SOLVER_ABORT. + // + // The user's parameter blocks will not be updated. + USER_FAILURE +}; + +// Enums used by the IterationCallback instances to indicate to the +// solver whether it should continue solving, the user detected an +// error or the solution is good enough and the solver should +// terminate. +enum CallbackReturnType { + // Continue solving to next iteration. + SOLVER_CONTINUE, + + // Terminate solver, and do not update the parameter blocks upon + // return. Unless the user has set + // Solver:Options:::update_state_every_iteration, in which case the + // state would have been updated every iteration + // anyways. Solver::Summary::termination_type is set to USER_ABORT. + SOLVER_ABORT, + + // Terminate solver, update state and + // return. Solver::Summary::termination_type is set to USER_SUCCESS. + SOLVER_TERMINATE_SUCCESSFULLY +}; + +// The format in which linear least squares problems should be logged +// when Solver::Options::lsqp_iterations_to_dump is non-empty. +enum DumpFormatType { + // Print the linear least squares problem in a human readable format + // to stderr. The Jacobian is printed as a dense matrix. The vectors + // D, x and f are printed as dense vectors. This should only be used + // for small problems. + CONSOLE, + + // Write out the linear least squares problem to the directory + // pointed to by Solver::Options::lsqp_dump_directory as text files + // which can be read into MATLAB/Octave. The Jacobian is dumped as a + // text file containing (i,j,s) triplets, the vectors D, x and f are + // dumped as text files containing a list of their values. + // + // A MATLAB/octave script called lm_iteration_???.m is also output, + // which can be used to parse and load the problem into memory. + TEXTFILE +}; + +// For SizedCostFunction and AutoDiffCostFunction, DYNAMIC can be +// specified for the number of residuals. If specified, then the +// number of residuas for that cost function can vary at runtime. +enum DimensionType { + DYNAMIC = -1 +}; + +// The differentiation method used to compute numerical derivatives in +// NumericDiffCostFunction and DynamicNumericDiffCostFunction. +enum NumericDiffMethodType { + // Compute central finite difference: f'(x) ~ (f(x+h) - f(x-h)) / 2h. + CENTRAL, + + // Compute forward finite difference: f'(x) ~ (f(x+h) - f(x)) / h. + FORWARD, + + // Adaptive numerical differentiation using Ridders' method. Provides more + // accurate and robust derivatives at the expense of additional cost + // function evaluations. + RIDDERS +}; + +enum LineSearchInterpolationType { + BISECTION, + QUADRATIC, + CUBIC +}; + +enum CovarianceAlgorithmType { + DENSE_SVD, + SUITE_SPARSE_QR, + EIGEN_SPARSE_QR +}; + +CERES_EXPORT const char* LinearSolverTypeToString( + LinearSolverType type); +CERES_EXPORT bool StringToLinearSolverType(std::string value, + LinearSolverType* type); + +CERES_EXPORT const char* PreconditionerTypeToString(PreconditionerType type); +CERES_EXPORT bool StringToPreconditionerType(std::string value, + PreconditionerType* type); + +CERES_EXPORT const char* VisibilityClusteringTypeToString( + VisibilityClusteringType type); +CERES_EXPORT bool StringToVisibilityClusteringType(std::string value, + VisibilityClusteringType* type); + +CERES_EXPORT const char* SparseLinearAlgebraLibraryTypeToString( + SparseLinearAlgebraLibraryType type); +CERES_EXPORT bool StringToSparseLinearAlgebraLibraryType( + std::string value, + SparseLinearAlgebraLibraryType* type); + +CERES_EXPORT const char* DenseLinearAlgebraLibraryTypeToString( + DenseLinearAlgebraLibraryType type); +CERES_EXPORT bool StringToDenseLinearAlgebraLibraryType( + std::string value, + DenseLinearAlgebraLibraryType* type); + +CERES_EXPORT const char* TrustRegionStrategyTypeToString( + TrustRegionStrategyType type); +CERES_EXPORT bool StringToTrustRegionStrategyType(std::string value, + TrustRegionStrategyType* type); + +CERES_EXPORT const char* DoglegTypeToString(DoglegType type); +CERES_EXPORT bool StringToDoglegType(std::string value, DoglegType* type); + +CERES_EXPORT const char* MinimizerTypeToString(MinimizerType type); +CERES_EXPORT bool StringToMinimizerType(std::string value, MinimizerType* type); + +CERES_EXPORT const char* LineSearchDirectionTypeToString( + LineSearchDirectionType type); +CERES_EXPORT bool StringToLineSearchDirectionType(std::string value, + LineSearchDirectionType* type); + +CERES_EXPORT const char* LineSearchTypeToString(LineSearchType type); +CERES_EXPORT bool StringToLineSearchType(std::string value, LineSearchType* type); + +CERES_EXPORT const char* NonlinearConjugateGradientTypeToString( + NonlinearConjugateGradientType type); +CERES_EXPORT bool StringToNonlinearConjugateGradientType( + std::string value, + NonlinearConjugateGradientType* type); + +CERES_EXPORT const char* LineSearchInterpolationTypeToString( + LineSearchInterpolationType type); +CERES_EXPORT bool StringToLineSearchInterpolationType( + std::string value, + LineSearchInterpolationType* type); + +CERES_EXPORT const char* CovarianceAlgorithmTypeToString( + CovarianceAlgorithmType type); +CERES_EXPORT bool StringToCovarianceAlgorithmType( + std::string value, + CovarianceAlgorithmType* type); + +CERES_EXPORT const char* NumericDiffMethodTypeToString( + NumericDiffMethodType type); +CERES_EXPORT bool StringToNumericDiffMethodType( + std::string value, + NumericDiffMethodType* type); + +CERES_EXPORT const char* TerminationTypeToString(TerminationType type); + +CERES_EXPORT bool IsSchurType(LinearSolverType type); +CERES_EXPORT bool IsSparseLinearAlgebraLibraryTypeAvailable( + SparseLinearAlgebraLibraryType type); +CERES_EXPORT bool IsDenseLinearAlgebraLibraryTypeAvailable( + DenseLinearAlgebraLibraryType type); + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_TYPES_H_ diff --git a/extern/ceres/include/ceres/version.h b/extern/ceres/include/ceres/version.h new file mode 100644 index 00000000000..66505a515c9 --- /dev/null +++ b/extern/ceres/include/ceres/version.h @@ -0,0 +1,48 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) + +#ifndef CERES_PUBLIC_VERSION_H_ +#define CERES_PUBLIC_VERSION_H_ + +#define CERES_VERSION_MAJOR 1 +#define CERES_VERSION_MINOR 11 +#define CERES_VERSION_REVISION 0 + +// Classic CPP stringifcation; the extra level of indirection allows the +// preprocessor to expand the macro before being converted to a string. +#define CERES_TO_STRING_HELPER(x) #x +#define CERES_TO_STRING(x) CERES_TO_STRING_HELPER(x) + +// The Ceres version as a string; for example "1.9.0". +#define CERES_VERSION_STRING CERES_TO_STRING(CERES_VERSION_MAJOR) "." \ + CERES_TO_STRING(CERES_VERSION_MINOR) "." \ + CERES_TO_STRING(CERES_VERSION_REVISION) + +#endif // CERES_PUBLIC_VERSION_H_ diff --git a/extern/ceres/internal/ceres/array_utils.cc b/extern/ceres/internal/ceres/array_utils.cc new file mode 100644 index 00000000000..7be3c78ce24 --- /dev/null +++ b/extern/ceres/internal/ceres/array_utils.cc @@ -0,0 +1,115 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/array_utils.h" + +#include <algorithm> +#include <cmath> +#include <cstddef> +#include <string> +#include <vector> +#include "ceres/fpclassify.h" +#include "ceres/stringprintf.h" + +namespace ceres { +namespace internal { + +using std::string; + +// It is a near impossibility that user code generates this exact +// value in normal operation, thus we will use it to fill arrays +// before passing them to user code. If on return an element of the +// array still contains this value, we will assume that the user code +// did not write to that memory location. +const double kImpossibleValue = 1e302; + +bool IsArrayValid(const int size, const double* x) { + if (x != NULL) { + for (int i = 0; i < size; ++i) { + if (!IsFinite(x[i]) || (x[i] == kImpossibleValue)) { + return false; + } + } + } + return true; +} + +int FindInvalidValue(const int size, const double* x) { + if (x == NULL) { + return size; + } + + for (int i = 0; i < size; ++i) { + if (!IsFinite(x[i]) || (x[i] == kImpossibleValue)) { + return i; + } + } + + return size; +} + +void InvalidateArray(const int size, double* x) { + if (x != NULL) { + for (int i = 0; i < size; ++i) { + x[i] = kImpossibleValue; + } + } +} + +void AppendArrayToString(const int size, const double* x, string* result) { + for (int i = 0; i < size; ++i) { + if (x == NULL) { + StringAppendF(result, "Not Computed "); + } else { + if (x[i] == kImpossibleValue) { + StringAppendF(result, "Uninitialized "); + } else { + StringAppendF(result, "%12g ", x[i]); + } + } + } +} + +void MapValuesToContiguousRange(const int size, int* array) { + std::vector<int> unique_values(array, array + size); + std::sort(unique_values.begin(), unique_values.end()); + unique_values.erase(std::unique(unique_values.begin(), + unique_values.end()), + unique_values.end()); + + for (int i = 0; i < size; ++i) { + array[i] = std::lower_bound(unique_values.begin(), + unique_values.end(), + array[i]) - unique_values.begin(); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/array_utils.h b/extern/ceres/internal/ceres/array_utils.h new file mode 100644 index 00000000000..2d2ffca8809 --- /dev/null +++ b/extern/ceres/internal/ceres/array_utils.h @@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Utility routines for validating arrays. +// +// These are useful for detecting two common class of errors. +// +// 1. Uninitialized memory - where the user for some reason did not +// compute part of an array, but the code expects it. +// +// 2. Numerical failure while computing the cost/residual/jacobian, +// e.g. NaN, infinities etc. This is particularly useful since the +// automatic differentiation code does computations that are not +// evident to the user and can silently generate hard to debug errors. + +#ifndef CERES_INTERNAL_ARRAY_UTILS_H_ +#define CERES_INTERNAL_ARRAY_UTILS_H_ + +#include <string> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +// Fill the array x with an impossible value that the user code is +// never expected to compute. +void InvalidateArray(int size, double* x); + +// Check if all the entries of the array x are valid, i.e. all the +// values in the array should be finite and none of them should be +// equal to the "impossible" value used by InvalidateArray. +bool IsArrayValid(int size, const double* x); + +// If the array contains an invalid value, return the index for it, +// otherwise return size. +int FindInvalidValue(const int size, const double* x); + +// Utility routine to print an array of doubles to a string. If the +// array pointer is NULL, it is treated as an array of zeros. +void AppendArrayToString(const int size, const double* x, std::string* result); + +extern const double kImpossibleValue; + +// This routine takes an array of integer values, sorts and uniques +// them and then maps each value in the array to its position in the +// sorted+uniqued array. By doing this, if there are are k unique +// values in the array, each value is replaced by an integer in the +// range [0, k-1], while preserving their relative order. +// +// For example +// +// [1 0 3 5 0 1 5] +// +// gets mapped to +// +// [1 0 2 3 0 1 3] +void MapValuesToContiguousRange(int size, int* array); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_ARRAY_UTILS_H_ diff --git a/extern/ceres/internal/ceres/blas.cc b/extern/ceres/internal/ceres/blas.cc new file mode 100644 index 00000000000..3ba63bbed5a --- /dev/null +++ b/extern/ceres/internal/ceres/blas.cc @@ -0,0 +1,81 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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 "ceres/internal/port.h" +#include "glog/logging.h" + +#ifndef CERES_NO_LAPACK +extern "C" void dsyrk_(char* uplo, + char* trans, + int* n, + int* k, + double* alpha, + double* a, + int* lda, + double* beta, + double* c, + int* ldc); +#endif + +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/ceres/internal/ceres/blas.h b/extern/ceres/internal/ceres/blas.h new file mode 100644 index 00000000000..a43301c5d18 --- /dev/null +++ b/extern/ceres/internal/ceres/blas.h @@ -0,0 +1,57 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// Wrapper functions around BLAS functions. + +#ifndef CERES_INTERNAL_BLAS_H_ +#define CERES_INTERNAL_BLAS_H_ + +namespace ceres { +namespace internal { + +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 + +#endif // CERES_INTERNAL_BLAS_H_ diff --git a/extern/ceres/internal/ceres/block_evaluate_preparer.cc b/extern/ceres/internal/ceres/block_evaluate_preparer.cc new file mode 100644 index 00000000000..59c0d3ecc10 --- /dev/null +++ b/extern/ceres/internal/ceres/block_evaluate_preparer.cc @@ -0,0 +1,83 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/block_evaluate_preparer.h" + +#include <vector> +#include "ceres/block_sparse_matrix.h" +#include "ceres/casts.h" +#include "ceres/parameter_block.h" +#include "ceres/residual_block.h" +#include "ceres/sparse_matrix.h" + +namespace ceres { +namespace internal { + +void BlockEvaluatePreparer::Init(int const* const* jacobian_layout, + int max_derivatives_per_residual_block) { + jacobian_layout_ = jacobian_layout; + scratch_evaluate_preparer_.Init(max_derivatives_per_residual_block); +} + +// Point the jacobian blocks directly into the block sparse matrix. +void BlockEvaluatePreparer::Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians) { + // If the overall jacobian is not available, use the scratch space. + if (jacobian == NULL) { + scratch_evaluate_preparer_.Prepare(residual_block, + residual_block_index, + jacobian, + jacobians); + return; + } + + double* jacobian_values = + down_cast<BlockSparseMatrix*>(jacobian)->mutable_values(); + + const int* jacobian_block_offset = jacobian_layout_[residual_block_index]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + if (!residual_block->parameter_blocks()[j]->IsConstant()) { + jacobians[j] = jacobian_values + *jacobian_block_offset; + + // The jacobian_block_offset can't be indexed with 'j' since the code + // that creates the layout strips out any blocks for inactive + // parameters. Instead, bump the pointer for active parameters only. + jacobian_block_offset++; + } else { + jacobians[j] = NULL; + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/block_evaluate_preparer.h b/extern/ceres/internal/ceres/block_evaluate_preparer.h new file mode 100644 index 00000000000..4378689729f --- /dev/null +++ b/extern/ceres/internal/ceres/block_evaluate_preparer.h @@ -0,0 +1,77 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A evaluate preparer which puts jacobian the evaluated jacobian blocks +// directly into their final resting place in an overall block sparse matrix. +// The evaluator takes care to avoid evaluating the jacobian for fixed +// parameters. + +#ifndef CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ +#define CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ + +#include "ceres/scratch_evaluate_preparer.h" + +namespace ceres { +namespace internal { + +class ResidualBlock; +class SparseMatrix; + +class BlockEvaluatePreparer { + public: + // Using Init() instead of a constructor allows for allocating this structure + // with new[]. This is because C++ doesn't allow passing arguments to objects + // constructed with new[] (as opposed to plain 'new'). + void Init(int const* const* jacobian_layout, + int max_derivatives_per_residual_block); + + // EvaluatePreparer interface + + // Point the jacobian blocks directly into the block sparse matrix, if + // jacobian is non-null. Otherwise, uses an internal per-thread buffer to + // store the jacobians temporarily. + void Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians); + + private: + int const* const* jacobian_layout_; + + // For the case that the overall jacobian is not available, but the + // individual jacobians are requested, use a pass-through scratch evaluate + // preparer. + ScratchEvaluatePreparer scratch_evaluate_preparer_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ diff --git a/extern/ceres/internal/ceres/block_jacobi_preconditioner.cc b/extern/ceres/internal/ceres/block_jacobi_preconditioner.cc new file mode 100644 index 00000000000..22d4b351c51 --- /dev/null +++ b/extern/ceres/internal/ceres/block_jacobi_preconditioner.cc @@ -0,0 +1,106 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/block_jacobi_preconditioner.h" + +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/block_random_access_diagonal_matrix.h" +#include "ceres/casts.h" +#include "ceres/integral_types.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +BlockJacobiPreconditioner::BlockJacobiPreconditioner( + const BlockSparseMatrix& A) { + const CompressedRowBlockStructure* bs = A.block_structure(); + std::vector<int> blocks(bs->cols.size()); + for (int i = 0; i < blocks.size(); ++i) { + blocks[i] = bs->cols[i].size; + } + + m_.reset(new BlockRandomAccessDiagonalMatrix(blocks)); +} + +BlockJacobiPreconditioner::~BlockJacobiPreconditioner() {} + +bool BlockJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, + const double* D) { + const CompressedRowBlockStructure* bs = A.block_structure(); + const double* values = A.values(); + m_->SetZero(); + for (int i = 0; i < bs->rows.size(); ++i) { + const int row_block_size = bs->rows[i].block.size; + const std::vector<Cell>& cells = bs->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + const int block_id = cells[j].block_id; + const int col_block_size = bs->cols[block_id].size; + + int r, c, row_stride, col_stride; + CellInfo* cell_info = m_->GetCell(block_id, block_id, + &r, &c, + &row_stride, &col_stride); + MatrixRef m(cell_info->values, row_stride, col_stride); + ConstMatrixRef b(values + cells[j].position, + row_block_size, + col_block_size); + m.block(r, c, col_block_size, col_block_size) += b.transpose() * b; + } + } + + if (D != NULL) { + // Add the diagonal. + int position = 0; + for (int i = 0; i < bs->cols.size(); ++i) { + const int block_size = bs->cols[i].size; + int r, c, row_stride, col_stride; + CellInfo* cell_info = m_->GetCell(i, i, + &r, &c, + &row_stride, &col_stride); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size, block_size).diagonal() += + ConstVectorRef(D + position, block_size).array().square().matrix(); + position += block_size; + } + } + + m_->Invert(); + return true; +} + +void BlockJacobiPreconditioner::RightMultiply(const double* x, + double* y) const { + m_->RightMultiply(x, y); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/block_jacobi_preconditioner.h b/extern/ceres/internal/ceres/block_jacobi_preconditioner.h new file mode 100644 index 00000000000..14007295823 --- /dev/null +++ b/extern/ceres/internal/ceres/block_jacobi_preconditioner.h @@ -0,0 +1,76 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ +#define CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ + +#include <vector> +#include "ceres/block_random_access_diagonal_matrix.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/preconditioner.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; +struct CompressedRowBlockStructure; + +// A block Jacobi preconditioner. This is intended for use with +// conjugate gradients, or other iterative symmetric solvers. To use +// the preconditioner, create one by passing a BlockSparseMatrix "A" +// to the constructor. This fixes the sparsity pattern to the pattern +// of the matrix A^TA. +// +// Before each use of the preconditioner in a solve with conjugate gradients, +// update the matrix by running Update(A, D). The values of the matrix A are +// inspected to construct the preconditioner. The vector D is applied as the +// D^TD diagonal term. +class BlockJacobiPreconditioner : public BlockSparseMatrixPreconditioner { + public: + // A must remain valid while the BlockJacobiPreconditioner is. + explicit BlockJacobiPreconditioner(const BlockSparseMatrix& A); + virtual ~BlockJacobiPreconditioner(); + + // Preconditioner interface + virtual void RightMultiply(const double* x, double* y) const; + virtual int num_rows() const { return m_->num_rows(); } + virtual int num_cols() const { return m_->num_rows(); } + + const BlockRandomAccessDiagonalMatrix& matrix() const { return *m_; } + private: + virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); + + scoped_ptr<BlockRandomAccessDiagonalMatrix> m_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ diff --git a/extern/ceres/internal/ceres/block_jacobian_writer.cc b/extern/ceres/internal/ceres/block_jacobian_writer.cc new file mode 100644 index 00000000000..7a3fee4fbdf --- /dev/null +++ b/extern/ceres/internal/ceres/block_jacobian_writer.cc @@ -0,0 +1,214 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/block_jacobian_writer.h" + +#include "ceres/block_evaluate_preparer.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +using std::vector; + +namespace { + +// Given the residual block ordering, build a lookup table to determine which +// per-parameter jacobian goes where in the overall program jacobian. +// +// Since we expect to use a Schur type linear solver to solve the LM step, take +// extra care to place the E blocks and the F blocks contiguously. E blocks are +// the first num_eliminate_blocks parameter blocks as indicated by the parameter +// block ordering. The remaining parameter blocks are the F blocks. +// +// TODO(keir): Consider if we should use a boolean for each parameter block +// instead of num_eliminate_blocks. +void BuildJacobianLayout(const Program& program, + int num_eliminate_blocks, + vector<int*>* jacobian_layout, + vector<int>* jacobian_layout_storage) { + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + + // Iterate over all the active residual blocks and determine how many E blocks + // are there. This will determine where the F blocks start in the jacobian + // matrix. Also compute the number of jacobian blocks. + int f_block_pos = 0; + int num_jacobian_blocks = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Advance f_block_pos over each E block for this residual. + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + // Only count blocks for active parameters. + num_jacobian_blocks++; + if (parameter_block->index() < num_eliminate_blocks) { + f_block_pos += num_residuals * parameter_block->LocalSize(); + } + } + } + } + + // We now know that the E blocks are laid out starting at zero, and the F + // blocks are laid out starting at f_block_pos. Iterate over the residual + // blocks again, and this time fill the jacobian_layout array with the + // position information. + + jacobian_layout->resize(program.NumResidualBlocks()); + jacobian_layout_storage->resize(num_jacobian_blocks); + + int e_block_pos = 0; + int* jacobian_pos = &(*jacobian_layout_storage)[0]; + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + (*jacobian_layout)[i] = jacobian_pos; + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + const int parameter_block_index = parameter_block->index(); + if (parameter_block->IsConstant()) { + continue; + } + const int jacobian_block_size = + num_residuals * parameter_block->LocalSize(); + if (parameter_block_index < num_eliminate_blocks) { + *jacobian_pos = e_block_pos; + e_block_pos += jacobian_block_size; + } else { + *jacobian_pos = f_block_pos; + f_block_pos += jacobian_block_size; + } + jacobian_pos++; + } + } +} + +} // namespace + +BlockJacobianWriter::BlockJacobianWriter(const Evaluator::Options& options, + Program* program) + : program_(program) { + CHECK_GE(options.num_eliminate_blocks, 0) + << "num_eliminate_blocks must be greater than 0."; + + BuildJacobianLayout(*program, + options.num_eliminate_blocks, + &jacobian_layout_, + &jacobian_layout_storage_); +} + +// Create evaluate prepareres that point directly into the final jacobian. This +// makes the final Write() a nop. +BlockEvaluatePreparer* BlockJacobianWriter::CreateEvaluatePreparers( + int num_threads) { + int max_derivatives_per_residual_block = + program_->MaxDerivativesPerResidualBlock(); + + BlockEvaluatePreparer* preparers = new BlockEvaluatePreparer[num_threads]; + for (int i = 0; i < num_threads; i++) { + preparers[i].Init(&jacobian_layout_[0], max_derivatives_per_residual_block); + } + return preparers; +} + +SparseMatrix* BlockJacobianWriter::CreateJacobian() const { + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + + const vector<ParameterBlock*>& parameter_blocks = + program_->parameter_blocks(); + + // Construct the column blocks. + bs->cols.resize(parameter_blocks.size()); + for (int i = 0, cursor = 0; i < parameter_blocks.size(); ++i) { + CHECK_NE(parameter_blocks[i]->index(), -1); + CHECK(!parameter_blocks[i]->IsConstant()); + bs->cols[i].size = parameter_blocks[i]->LocalSize(); + bs->cols[i].position = cursor; + cursor += bs->cols[i].size; + } + + // Construct the cells in each row. + const vector<ResidualBlock*>& residual_blocks = program_->residual_blocks(); + int row_block_position = 0; + bs->rows.resize(residual_blocks.size()); + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + CompressedRow* row = &bs->rows[i]; + + row->block.size = residual_block->NumResiduals(); + row->block.position = row_block_position; + row_block_position += row->block.size; + + // Size the row by the number of active parameters in this residual. + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + int num_active_parameter_blocks = 0; + for (int j = 0; j < num_parameter_blocks; ++j) { + if (residual_block->parameter_blocks()[j]->index() != -1) { + num_active_parameter_blocks++; + } + } + row->cells.resize(num_active_parameter_blocks); + + // Add layout information for the active parameters in this row. + for (int j = 0, k = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + Cell& cell = row->cells[k]; + cell.block_id = parameter_block->index(); + cell.position = jacobian_layout_[i][k]; + + // Only increment k for active parameters, since there is only layout + // information for active parameters. + k++; + } + } + + sort(row->cells.begin(), row->cells.end(), CellLessThan); + } + + BlockSparseMatrix* jacobian = new BlockSparseMatrix(bs); + CHECK_NOTNULL(jacobian); + return jacobian; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/block_jacobian_writer.h b/extern/ceres/internal/ceres/block_jacobian_writer.h new file mode 100644 index 00000000000..8e6f45130a4 --- /dev/null +++ b/extern/ceres/internal/ceres/block_jacobian_writer.h @@ -0,0 +1,127 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A jacobian writer that writes to block sparse matrices. The "writer" name is +// misleading, since the Write() operation on the block jacobian writer does not +// write anything. Instead, the Prepare() method on the BlockEvaluatePreparers +// makes a jacobians array which has direct pointers into the block sparse +// jacobian. When the cost function is evaluated, the jacobian blocks get placed +// directly in their final location. + +#ifndef CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ + +#include <vector> +#include "ceres/evaluator.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class BlockEvaluatePreparer; +class Program; +class SparseMatrix; + +class BlockJacobianWriter { + public: + BlockJacobianWriter(const Evaluator::Options& options, + Program* program); + + // JacobianWriter interface. + + // Create evaluate prepareres that point directly into the final jacobian. + // This makes the final Write() a nop. + BlockEvaluatePreparer* CreateEvaluatePreparers(int num_threads); + + SparseMatrix* CreateJacobian() const; + + void Write(int /* residual_id */, + int /* residual_offset */, + double** /* jacobians */, + SparseMatrix* /* jacobian */) { + // This is a noop since the blocks were written directly into their final + // position by the outside evaluate call, thanks to the jacobians array + // prepared by the BlockEvaluatePreparers. + } + + private: + Program* program_; + + // Stores the position of each residual / parameter jacobian. + // + // The block sparse matrix that this writer writes to is stored as a set of + // contiguos dense blocks, one after each other; see BlockSparseMatrix. The + // "double* values_" member of the block sparse matrix contains all of these + // blocks. Given a pointer to the first element of a block and the size of + // that block, it's possible to write to it. + // + // In the case of a block sparse jacobian, the jacobian writer needs a way to + // find the offset in the values_ array of each residual/parameter jacobian + // block. + // + // That is the purpose of jacobian_layout_. + // + // In particular, jacobian_layout_[i][j] is the offset in the values_ array of + // the derivative of residual block i with respect to the parameter block at + // active argument position j. + // + // The active qualifier means that non-active parameters do not count. Care + // must be taken when indexing into jacobian_layout_ to account for this. + // Consider a single residual example: + // + // r(x, y, z) + // + // with r in R^3, x in R^4, y in R^2, and z in R^5. + // Take y as a constant (non-active) parameter. + // Take r as residual number 0. + // + // In this case, the active arguments are only (x, z), so the active argument + // position for x is 0, and the active argument position for z is 1. This is + // similar to thinking of r as taking only 2 parameters: + // + // r(x, z) + // + // There are only 2 jacobian blocks: dr/dx and dr/dz. jacobian_layout_ would + // have the following contents: + // + // jacobian_layout_[0] = { 0, 12 } + // + // which indicates that dr/dx is located at values_[0], and dr/dz is at + // values_[12]. See BlockEvaluatePreparer::Prepare()'s comments about 'j'. + std::vector<int*> jacobian_layout_; + + // The pointers in jacobian_layout_ point directly into this vector. + std::vector<int> jacobian_layout_storage_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ diff --git a/extern/ceres/internal/ceres/block_random_access_dense_matrix.cc b/extern/ceres/internal/ceres/block_random_access_dense_matrix.cc new file mode 100644 index 00000000000..61748ef6f7f --- /dev/null +++ b/extern/ceres/internal/ceres/block_random_access_dense_matrix.cc @@ -0,0 +1,88 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_dense_matrix.h" + +#include <vector> +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessDenseMatrix::BlockRandomAccessDenseMatrix( + const std::vector<int>& blocks) { + const int num_blocks = blocks.size(); + block_layout_.resize(num_blocks, 0); + num_rows_ = 0; + for (int i = 0; i < num_blocks; ++i) { + block_layout_[i] = num_rows_; + num_rows_ += blocks[i]; + } + + values_.reset(new double[num_rows_ * num_rows_]); + + cell_infos_.reset(new CellInfo[num_blocks * num_blocks]); + for (int i = 0; i < num_blocks * num_blocks; ++i) { + cell_infos_[i].values = values_.get(); + } + + SetZero(); +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +BlockRandomAccessDenseMatrix::~BlockRandomAccessDenseMatrix() { +} + +CellInfo* BlockRandomAccessDenseMatrix::GetCell(const int row_block_id, + const int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) { + *row = block_layout_[row_block_id]; + *col = block_layout_[col_block_id]; + *row_stride = num_rows_; + *col_stride = num_rows_; + return &cell_infos_[row_block_id * block_layout_.size() + col_block_id]; +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +void BlockRandomAccessDenseMatrix::SetZero() { + if (num_rows_) { + VectorRef(values_.get(), num_rows_ * num_rows_).setZero(); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/block_random_access_dense_matrix.h b/extern/ceres/internal/ceres/block_random_access_dense_matrix.h new file mode 100644 index 00000000000..89689082561 --- /dev/null +++ b/extern/ceres/internal/ceres/block_random_access_dense_matrix.h @@ -0,0 +1,98 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ + +#include "ceres/block_random_access_matrix.h" + +#include <vector> + +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// A square block random accessible matrix with the same row and +// column block structure. All cells are stored in the same single +// array, so that its also accessible as a dense matrix of size +// num_rows x num_cols. +// +// This class is NOT thread safe. Since all n^2 cells are stored, +// GetCell never returns NULL for any (row_block_id, col_block_id) +// pair. +// +// ReturnCell is a nop. +class BlockRandomAccessDenseMatrix : public BlockRandomAccessMatrix { + public: + // blocks is a vector of block sizes. The resulting matrix has + // blocks.size() * blocks.size() cells. + explicit BlockRandomAccessDenseMatrix(const std::vector<int>& blocks); + + // The destructor is not thread safe. It assumes that no one is + // modifying any cells when the matrix is being destroyed. + virtual ~BlockRandomAccessDenseMatrix(); + + // BlockRandomAccessMatrix interface. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride); + + // This is not a thread safe method, it assumes that no cell is + // locked. + virtual void SetZero(); + + // Since the matrix is square with the same row and column block + // structure, num_rows() = num_cols(). + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_rows_; } + + // The underlying matrix storing the cells. + const double* values() const { return values_.get(); } + double* mutable_values() { return values_.get(); } + + private: + int num_rows_; + std::vector<int> block_layout_; + scoped_array<double> values_; + scoped_array<CellInfo> cell_infos_; + + CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessDenseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.cc b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.cc new file mode 100644 index 00000000000..052690d18be --- /dev/null +++ b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.cc @@ -0,0 +1,154 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_diagonal_matrix.h" + +#include <algorithm> +#include <set> +#include <utility> +#include <vector> +#include "Eigen/Dense" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/stl_util.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::vector; + +// TODO(sameeragarwal): Drop the dependence on TripletSparseMatrix. + +BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix( + const vector<int>& blocks) + : blocks_(blocks) { + // Build the row/column layout vector and count the number of scalar + // rows/columns. + int num_cols = 0; + int num_nonzeros = 0; + vector<int> block_positions; + for (int i = 0; i < blocks_.size(); ++i) { + block_positions.push_back(num_cols); + num_cols += blocks_[i]; + num_nonzeros += blocks_[i] * blocks_[i]; + } + + VLOG(1) << "Matrix Size [" << num_cols + << "," << num_cols + << "] " << num_nonzeros; + + tsm_.reset(new TripletSparseMatrix(num_cols, num_cols, num_nonzeros)); + tsm_->set_num_nonzeros(num_nonzeros); + int* rows = tsm_->mutable_rows(); + int* cols = tsm_->mutable_cols(); + double* values = tsm_->mutable_values(); + + int pos = 0; + for (int i = 0; i < blocks_.size(); ++i) { + const int block_size = blocks_[i]; + layout_.push_back(new CellInfo(values + pos)); + const int block_begin = block_positions[i]; + for (int r = 0; r < block_size; ++r) { + for (int c = 0; c < block_size; ++c, ++pos) { + rows[pos] = block_begin + r; + cols[pos] = block_begin + c; + } + } + } +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +BlockRandomAccessDiagonalMatrix::~BlockRandomAccessDiagonalMatrix() { + STLDeleteContainerPointers(layout_.begin(), layout_.end()); +} + +CellInfo* BlockRandomAccessDiagonalMatrix::GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) { + if (row_block_id != col_block_id) { + return NULL; + } + const int stride = blocks_[row_block_id]; + + // Each cell is stored contiguously as its own little dense matrix. + *row = 0; + *col = 0; + *row_stride = stride; + *col_stride = stride; + return layout_[row_block_id]; +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +void BlockRandomAccessDiagonalMatrix::SetZero() { + if (tsm_->num_nonzeros()) { + VectorRef(tsm_->mutable_values(), + tsm_->num_nonzeros()).setZero(); + } +} + +void BlockRandomAccessDiagonalMatrix::Invert() { + double* values = tsm_->mutable_values(); + for (int i = 0; i < blocks_.size(); ++i) { + const int block_size = blocks_[i]; + MatrixRef block(values, block_size, block_size); + block = + block + .selfadjointView<Eigen::Upper>() + .llt() + .solve(Matrix::Identity(block_size, block_size)); + values += block_size * block_size; + } +} + +void BlockRandomAccessDiagonalMatrix::RightMultiply(const double* x, + double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + const double* values = tsm_->values(); + for (int i = 0; i < blocks_.size(); ++i) { + const int block_size = blocks_[i]; + ConstMatrixRef block(values, block_size, block_size); + VectorRef(y, block_size).noalias() += block * ConstVectorRef(x, block_size); + x += block_size; + y += block_size; + values += block_size * block_size; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.h b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.h new file mode 100644 index 00000000000..07ffc9d4a0d --- /dev/null +++ b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.h @@ -0,0 +1,101 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_DIAGONAL_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DIAGONAL_MATRIX_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/mutex.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/collections_port.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/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 thread safe block diagonal matrix implementation of +// BlockRandomAccessMatrix. +class BlockRandomAccessDiagonalMatrix : public BlockRandomAccessMatrix { + public: + // blocks is an array of block sizes. + explicit BlockRandomAccessDiagonalMatrix(const std::vector<int>& blocks); + + // The destructor is not thread safe. It assumes that no one is + // modifying any cells when the matrix is being destroyed. + virtual ~BlockRandomAccessDiagonalMatrix(); + + // 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(); + + // Invert the matrix assuming that each block is positive definite. + void Invert(); + + // y += S * x + void RightMultiply(const double* x, double* y) const; + + // Since the matrix is square, num_rows() == num_cols(). + virtual int num_rows() const { return tsm_->num_rows(); } + virtual int num_cols() const { return tsm_->num_cols(); } + + const TripletSparseMatrix* matrix() const { return tsm_.get(); } + TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } + + private: + // row/column block sizes. + const std::vector<int> blocks_; + std::vector<CellInfo*> layout_; + + // The underlying matrix object which actually stores the cells. + scoped_ptr<TripletSparseMatrix> tsm_; + + friend class BlockRandomAccessDiagonalMatrixTest; + CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessDiagonalMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DIAGONAL_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/block_random_access_matrix.cc b/extern/ceres/internal/ceres/block_random_access_matrix.cc new file mode 100644 index 00000000000..347d765bbca --- /dev/null +++ b/extern/ceres/internal/ceres/block_random_access_matrix.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_matrix.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessMatrix::~BlockRandomAccessMatrix() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/block_random_access_matrix.h b/extern/ceres/internal/ceres/block_random_access_matrix.h new file mode 100644 index 00000000000..34c8bf5cd4d --- /dev/null +++ b/extern/ceres/internal/ceres/block_random_access_matrix.h @@ -0,0 +1,132 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Interface for matrices that allow block based random access. + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ + +#include "ceres/mutex.h" + +namespace ceres { +namespace internal { + +// A matrix implementing the BlockRandomAccessMatrix interface is a +// matrix whose rows and columns are divided into blocks. For example +// the matrix A: +// +// 3 4 5 +// A = 5 [c_11 c_12 c_13] +// 4 [c_21 c_22 c_23] +// +// has row blocks of size 5 and 4, and column blocks of size 3, 4 and +// 5. It has six cells corresponding to the six row-column block +// combinations. +// +// BlockRandomAccessMatrix objects provide access to cells c_ij using +// the GetCell method. when a cell is present, GetCell will return a +// CellInfo object containing a pointer to an array which contains the +// cell as a submatrix and a mutex that guards this submatrix. If the +// user is accessing the matrix concurrently, it is his responsibility +// to use the mutex to exclude other writers from writing to the cell +// concurrently. +// +// There is no requirement that all cells be present, i.e. the matrix +// itself can be block sparse. When a cell is not present, the GetCell +// method will return a NULL pointer. +// +// There is no requirement about how the cells are stored beyond that +// form a dense submatrix of a larger dense matrix. Like everywhere +// else in Ceres, RowMajor storage assumed. +// +// Example usage: +// +// BlockRandomAccessMatrix* A = new BlockRandomAccessMatrixSubClass(...) +// +// int row, col, row_stride, col_stride; +// CellInfo* cell = A->GetCell(row_block_id, col_block_id, +// &row, &col, +// &row_stride, &col_stride); +// +// if (cell != NULL) { +// MatrixRef m(cell->values, row_stride, col_stride); +// CeresMutexLock l(&cell->m); +// m.block(row, col, row_block_size, col_block_size) = ... +// } + +// Structure to carry a pointer to the array containing a cell and the +// Mutex guarding it. +struct CellInfo { + CellInfo() + : values(NULL) { + } + + explicit CellInfo(double* ptr) + : values(ptr) { + } + + double* values; + Mutex m; +}; + +class BlockRandomAccessMatrix { + public: + virtual ~BlockRandomAccessMatrix(); + + // If the cell (row_block_id, col_block_id) is present, then return + // a CellInfo with a pointer to the dense matrix containing it, + // otherwise return NULL. The dense matrix containing this cell has + // size row_stride, col_stride and the cell is located at position + // (row, col) within this matrix. + // + // The size of the cell is row_block_size x col_block_size is + // assumed known to the caller. row_block_size less than or equal to + // row_stride and col_block_size is upper bounded by col_stride. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) = 0; + + // Zero out the values of the array. The structure of the matrix + // (size and sparsity) is preserved. + virtual void SetZero() = 0; + + // Number of scalar rows and columns in the matrix, i.e the sum of + // all row blocks and column block sizes respectively. + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/block_random_access_sparse_matrix.cc b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.cc new file mode 100644 index 00000000000..5432ec1064a --- /dev/null +++ b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.cc @@ -0,0 +1,196 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_sparse_matrix.h" + +#include <algorithm> +#include <set> +#include <utility> +#include <vector> +#include "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 { + +using std::make_pair; +using std::pair; +using std::set; +using std::vector; + +BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( + const vector<int>& blocks, + const set<pair<int, int> >& block_pairs) + : kMaxRowBlocks(10 * 1000 * 1000), + blocks_(blocks) { + CHECK_LT(blocks.size(), kMaxRowBlocks); + + // Build the row/column layout vector and count the number of scalar + // rows/columns. + int num_cols = 0; + block_positions_.reserve(blocks_.size()); + for (int i = 0; i < blocks_.size(); ++i) { + block_positions_.push_back(num_cols); + num_cols += blocks_[i]; + } + + // Count the number of scalar non-zero entries and build the layout + // object for looking into the values array of the + // TripletSparseMatrix. + int num_nonzeros = 0; + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_size = blocks_[it->first]; + const int col_block_size = blocks_[it->second]; + num_nonzeros += row_block_size * col_block_size; + } + + VLOG(1) << "Matrix Size [" << num_cols + << "," << num_cols + << "] " << num_nonzeros; + + tsm_.reset(new TripletSparseMatrix(num_cols, num_cols, num_nonzeros)); + tsm_->set_num_nonzeros(num_nonzeros); + int* rows = tsm_->mutable_rows(); + int* cols = tsm_->mutable_cols(); + double* values = tsm_->mutable_values(); + + int pos = 0; + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_size = blocks_[it->first]; + const int col_block_size = blocks_[it->second]; + cell_values_.push_back(make_pair(make_pair(it->first, it->second), + values + pos)); + layout_[IntPairToLong(it->first, it->second)] = + new CellInfo(values + pos); + pos += row_block_size * col_block_size; + } + + // Fill the sparsity pattern of the underlying matrix. + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_id = it->first; + const int col_block_id = it->second; + const int row_block_size = blocks_[row_block_id]; + const int col_block_size = blocks_[col_block_id]; + int pos = + layout_[IntPairToLong(row_block_id, col_block_id)]->values - values; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c, ++pos) { + rows[pos] = block_positions_[row_block_id] + r; + cols[pos] = block_positions_[col_block_id] + c; + values[pos] = 1.0; + DCHECK_LT(rows[pos], tsm_->num_rows()); + DCHECK_LT(cols[pos], tsm_->num_rows()); + } + } + } +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +BlockRandomAccessSparseMatrix::~BlockRandomAccessSparseMatrix() { + for (LayoutType::iterator it = layout_.begin(); + it != layout_.end(); + ++it) { + delete it->second; + } +} + +CellInfo* BlockRandomAccessSparseMatrix::GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) { + const LayoutType::iterator it = + layout_.find(IntPairToLong(row_block_id, col_block_id)); + if (it == layout_.end()) { + return NULL; + } + + // Each cell is stored contiguously as its own little dense matrix. + *row = 0; + *col = 0; + *row_stride = blocks_[row_block_id]; + *col_stride = blocks_[col_block_id]; + return it->second; +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +void BlockRandomAccessSparseMatrix::SetZero() { + if (tsm_->num_nonzeros()) { + VectorRef(tsm_->mutable_values(), + tsm_->num_nonzeros()).setZero(); + } +} + +void BlockRandomAccessSparseMatrix::SymmetricRightMultiply(const double* x, + double* y) const { + vector< pair<pair<int, int>, double*> >::const_iterator it = + cell_values_.begin(); + for (; it != cell_values_.end(); ++it) { + const int row = it->first.first; + const int row_block_size = blocks_[row]; + const int row_block_pos = block_positions_[row]; + + const int col = it->first.second; + const int col_block_size = blocks_[col]; + const int col_block_pos = block_positions_[col]; + + MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + it->second, row_block_size, col_block_size, + x + col_block_pos, + y + row_block_pos); + + // Since the matrix is symmetric, but only the upper triangular + // part is stored, if the block being accessed is not a diagonal + // block, then use the same block to do the corresponding lower + // triangular multiply also. + if (row != col) { + MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + it->second, row_block_size, col_block_size, + x + row_block_pos, + y + col_block_pos); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/block_random_access_sparse_matrix.h b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.h new file mode 100644 index 00000000000..2b3c7fdabae --- /dev/null +++ b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.h @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/mutex.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/collections_port.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/integral_types.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" +#include "ceres/small_blas.h" + +namespace ceres { +namespace internal { + +// A thread safe square block sparse implementation of +// BlockRandomAccessMatrix. Internally a TripletSparseMatrix is used +// for doing the actual storage. This class augments this matrix with +// an unordered_map that allows random read/write access. +class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { + public: + // blocks is an array of block sizes. block_pairs is a set of + // <row_block_id, col_block_id> pairs to identify the non-zero cells + // of this matrix. + BlockRandomAccessSparseMatrix( + const std::vector<int>& blocks, + const std::set<std::pair<int, int> >& block_pairs); + + // The destructor is not thread safe. It assumes that no one is + // modifying any cells when the matrix is being destroyed. + virtual ~BlockRandomAccessSparseMatrix(); + + // BlockRandomAccessMatrix Interface. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride); + + // This is not a thread safe method, it assumes that no cell is + // locked. + virtual void SetZero(); + + // Assume that the matrix is symmetric and only one half of the + // matrix is stored. + // + // y += S * x + void SymmetricRightMultiply(const double* x, double* y) const; + + // Since the matrix is square, num_rows() == num_cols(). + virtual int num_rows() const { return tsm_->num_rows(); } + virtual int num_cols() const { return tsm_->num_cols(); } + + // Access to the underlying matrix object. + const TripletSparseMatrix* matrix() const { return tsm_.get(); } + TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } + + private: + int64 IntPairToLong(int row, int col) const { + return row * kMaxRowBlocks + col; + } + + void LongToIntPair(int64 index, int* row, int* col) const { + *row = index / kMaxRowBlocks; + *col = index % kMaxRowBlocks; + } + + const int64 kMaxRowBlocks; + + // row/column block sizes. + const std::vector<int> blocks_; + std::vector<int> block_positions_; + + // 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_; + + // In order traversal of contents of the matrix. This allows us to + // implement a matrix-vector which is 20% faster than using the + // iterator in the Layout object instead. + std::vector<std::pair<std::pair<int, int>, double*> > cell_values_; + // The underlying matrix object which actually stores the cells. + scoped_ptr<TripletSparseMatrix> tsm_; + + friend class BlockRandomAccessSparseMatrixTest; + CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/block_sparse_matrix.cc b/extern/ceres/internal/ceres/block_sparse_matrix.cc new file mode 100644 index 00000000000..68d0780156c --- /dev/null +++ b/extern/ceres/internal/ceres/block_sparse_matrix.cc @@ -0,0 +1,246 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_sparse_matrix.h" + +#include <cstddef> +#include <algorithm> +#include <vector> +#include "ceres/block_structure.h" +#include "ceres/internal/eigen.h" +#include "ceres/small_blas.h" +#include "ceres/triplet_sparse_matrix.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::vector; + +BlockSparseMatrix::~BlockSparseMatrix() {} + +BlockSparseMatrix::BlockSparseMatrix( + CompressedRowBlockStructure* block_structure) + : num_rows_(0), + num_cols_(0), + num_nonzeros_(0), + values_(NULL), + block_structure_(block_structure) { + CHECK_NOTNULL(block_structure_.get()); + + // Count the number of columns in the matrix. + for (int i = 0; i < block_structure_->cols.size(); ++i) { + num_cols_ += block_structure_->cols[i].size; + } + + // Count the number of non-zero entries and the number of rows in + // the matrix. + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + num_rows_ += row_block_size; + + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + num_nonzeros_ += col_block_size * row_block_size; + } + } + + CHECK_GE(num_rows_, 0); + CHECK_GE(num_cols_, 0); + CHECK_GE(num_nonzeros_, 0); + VLOG(2) << "Allocating values array with " + << num_nonzeros_ * sizeof(double) << " bytes."; // NOLINT + values_.reset(new double[num_nonzeros_]); + CHECK_NOTNULL(values_.get()); +} + +void BlockSparseMatrix::SetZero() { + std::fill(values_.get(), values_.get() + num_nonzeros_, 0.0); +} + +void BlockSparseMatrix::RightMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + values_.get() + cells[j].position, row_block_size, col_block_size, + x + col_block_pos, + y + row_block_pos); + } + } +} + +void BlockSparseMatrix::LeftMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + values_.get() + cells[j].position, row_block_size, col_block_size, + x + row_block_pos, + y + col_block_pos); + } + } +} + +void BlockSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + VectorRef(x, num_cols_).setZero(); + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + const MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + VectorRef(x + col_block_pos, col_block_size) += m.colwise().squaredNorm(); + } + } +} + +void BlockSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + m *= ConstVectorRef(scale + col_block_pos, col_block_size).asDiagonal(); + } + } +} + +void BlockSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + CHECK_NOTNULL(dense_matrix); + + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + Matrix& m = *dense_matrix; + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + m.block(row_block_pos, col_block_pos, row_block_size, col_block_size) + += MatrixRef(values_.get() + jac_pos, row_block_size, col_block_size); + } + } +} + +void BlockSparseMatrix::ToTripletSparseMatrix( + TripletSparseMatrix* matrix) const { + CHECK_NOTNULL(matrix); + + matrix->Reserve(num_nonzeros_); + matrix->Resize(num_rows_, num_cols_); + matrix->SetZero(); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c, ++jac_pos) { + matrix->mutable_rows()[jac_pos] = row_block_pos + r; + matrix->mutable_cols()[jac_pos] = col_block_pos + c; + matrix->mutable_values()[jac_pos] = values_[jac_pos]; + } + } + } + } + matrix->set_num_nonzeros(num_nonzeros_); +} + +// Return a pointer to the block structure. We continue to hold +// ownership of the object though. +const CompressedRowBlockStructure* BlockSparseMatrix::block_structure() + const { + return block_structure_.get(); +} + +void BlockSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + for (int i = 0; i < block_structure_->rows.size(); ++i) { + const int row_block_pos = block_structure_->rows[i].block.position; + const int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + const int col_block_id = cells[j].block_id; + const int col_block_size = block_structure_->cols[col_block_id].size; + const int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c) { + fprintf(file, "% 10d % 10d %17f\n", + row_block_pos + r, + col_block_pos + c, + values_[jac_pos++]); + } + } + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/block_sparse_matrix.h b/extern/ceres/internal/ceres/block_sparse_matrix.h new file mode 100644 index 00000000000..2f9afb738f8 --- /dev/null +++ b/extern/ceres/internal/ceres/block_sparse_matrix.h @@ -0,0 +1,100 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Implementation of the SparseMatrix interface for block sparse +// matrices. + +#ifndef CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ + +#include "ceres/block_structure.h" +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +class TripletSparseMatrix; + +// This class implements the SparseMatrix interface for storing and +// manipulating block sparse matrices. The block structure is stored +// in the CompressedRowBlockStructure object and one is needed to +// initialize the matrix. For details on how the blocks structure of +// the matrix is stored please see the documentation +// +// internal/ceres/block_structure.h +// +class BlockSparseMatrix : public SparseMatrix { + public: + // Construct a block sparse matrix with a fully initialized + // CompressedRowBlockStructure objected. The matrix takes over + // ownership of this object and destroys it upon destruction. + // + // TODO(sameeragarwal): Add a function which will validate legal + // CompressedRowBlockStructure objects. + explicit BlockSparseMatrix(CompressedRowBlockStructure* block_structure); + + BlockSparseMatrix(); + virtual ~BlockSparseMatrix(); + + // Implementation of SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; + virtual void ToTextFile(FILE* file) const; + + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_cols_; } + virtual int num_nonzeros() const { return num_nonzeros_; } + virtual const double* values() const { return values_.get(); } + virtual double* mutable_values() { return values_.get(); } + + void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const; + const CompressedRowBlockStructure* block_structure() const; + + private: + int num_rows_; + int num_cols_; + int max_num_nonzeros_; + int num_nonzeros_; + scoped_array<double> values_; + scoped_ptr<CompressedRowBlockStructure> block_structure_; + CERES_DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/block_structure.cc b/extern/ceres/internal/ceres/block_structure.cc new file mode 100644 index 00000000000..6479b60f700 --- /dev/null +++ b/extern/ceres/internal/ceres/block_structure.cc @@ -0,0 +1,44 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_structure.h" + +namespace ceres { +namespace internal { + +bool CellLessThan(const Cell& lhs, const Cell& rhs) { + if (lhs.block_id == rhs.block_id) { + return (lhs.position < rhs.position); + } + return (lhs.block_id < rhs.block_id); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/block_structure.h b/extern/ceres/internal/ceres/block_structure.h new file mode 100644 index 00000000000..6e7003addb6 --- /dev/null +++ b/extern/ceres/internal/ceres/block_structure.h @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Block structure objects are used to carry information about the +// dense block structure of sparse matrices. The BlockSparseMatrix +// object uses the BlockStructure objects to keep track of the matrix +// structure and operate upon it. This allows us to use more cache +// friendly block oriented linear algebra operations on the matrix +// instead of accessing it one scalar entry at a time. + +#ifndef CERES_INTERNAL_BLOCK_STRUCTURE_H_ +#define CERES_INTERNAL_BLOCK_STRUCTURE_H_ + +#include <vector> +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +typedef int32 BlockSize; + +struct Block { + Block() : size(-1), position(-1) {} + Block(int size_, int position_) : size(size_), position(position_) {} + + BlockSize size; + int position; // Position along the row/column. +}; + +struct Cell { + Cell() : block_id(-1), position(-1) {} + Cell(int block_id_, int position_) + : block_id(block_id_), position(position_) {} + + // Column or row block id as the case maybe. + int block_id; + // Where in the values array of the jacobian is this cell located. + int position; +}; + +// Order cell by their block_id; +bool CellLessThan(const Cell& lhs, const Cell& rhs); + +struct CompressedList { + Block block; + std::vector<Cell> cells; +}; + +typedef CompressedList CompressedRow; +typedef CompressedList CompressedColumn; + +struct CompressedRowBlockStructure { + std::vector<Block> cols; + std::vector<CompressedRow> rows; +}; + +struct CompressedColumnBlockStructure { + std::vector<Block> rows; + std::vector<CompressedColumn> cols; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_STRUCTURE_H_ diff --git a/extern/ceres/internal/ceres/c_api.cc b/extern/ceres/internal/ceres/c_api.cc new file mode 100644 index 00000000000..ada8f3e0013 --- /dev/null +++ b/extern/ceres/internal/ceres/c_api.cc @@ -0,0 +1,188 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/ceres/internal/ceres/callbacks.cc b/extern/ceres/internal/ceres/callbacks.cc new file mode 100644 index 00000000000..50a0ec19924 --- /dev/null +++ b/extern/ceres/internal/ceres/callbacks.cc @@ -0,0 +1,111 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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 <iostream> // NO LINT +#include "ceres/callbacks.h" +#include "ceres/program.h" +#include "ceres/stringprintf.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::string; + +StateUpdatingCallback::StateUpdatingCallback(Program* program, + double* parameters) + : program_(program), parameters_(parameters) {} + +StateUpdatingCallback::~StateUpdatingCallback() {} + +CallbackReturnType StateUpdatingCallback::operator()( + const IterationSummary& summary) { + if (summary.step_is_successful) { + program_->StateVectorToParameterBlocks(parameters_); + program_->CopyParameterBlockStateToUserState(); + } + return SOLVER_CONTINUE; +} + +LoggingCallback::LoggingCallback(const MinimizerType minimizer_type, + const bool log_to_stdout) + : minimizer_type(minimizer_type), + log_to_stdout_(log_to_stdout) {} + +LoggingCallback::~LoggingCallback() {} + +CallbackReturnType LoggingCallback::operator()( + const IterationSummary& summary) { + string output; + if (minimizer_type == LINE_SEARCH) { + const char* kReportRowFormat = + "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " + "s:% 3.2e e:% 3d it:% 3.2e tt:% 3.2e"; + output = StringPrintf(kReportRowFormat, + summary.iteration, + summary.cost, + summary.cost_change, + summary.gradient_max_norm, + summary.step_norm, + summary.step_size, + summary.line_search_function_evaluations, + summary.iteration_time_in_seconds, + summary.cumulative_time_in_seconds); + } else if (minimizer_type == TRUST_REGION) { + if (summary.iteration == 0) { + output = "iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time\n"; // NOLINT + } + const char* kReportRowFormat = + "% 4d % 8e % 3.2e % 3.2e % 3.2e % 3.2e % 3.2e % 4d % 3.2e % 3.2e"; // NOLINT + output += StringPrintf(kReportRowFormat, + summary.iteration, + summary.cost, + summary.cost_change, + summary.gradient_max_norm, + summary.step_norm, + summary.relative_decrease, + summary.trust_region_radius, + summary.linear_solver_iterations, + summary.iteration_time_in_seconds, + summary.cumulative_time_in_seconds); + } else { + LOG(FATAL) << "Unknown minimizer type."; + } + + if (log_to_stdout_) { + std::cout << output << std::endl; + } else { + VLOG(1) << output; + } + return SOLVER_CONTINUE; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/callbacks.h b/extern/ceres/internal/ceres/callbacks.h new file mode 100644 index 00000000000..33c66df5c11 --- /dev/null +++ b/extern/ceres/internal/ceres/callbacks.h @@ -0,0 +1,71 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_CALLBACKS_H_ +#define CERES_INTERNAL_CALLBACKS_H_ + +#include <string> +#include "ceres/iteration_callback.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class Program; + +// Callback for updating the externally visible state of parameter +// blocks. +class StateUpdatingCallback : public IterationCallback { + public: + StateUpdatingCallback(Program* program, double* parameters); + virtual ~StateUpdatingCallback(); + virtual CallbackReturnType operator()(const IterationSummary& summary); + private: + Program* program_; + double* parameters_; +}; + +// Callback for logging the state of the minimizer to STDERR or +// STDOUT depending on the user's preferences and logging level. +class LoggingCallback : public IterationCallback { + public: + LoggingCallback(MinimizerType minimizer_type, bool log_to_stdout); + virtual ~LoggingCallback(); + virtual CallbackReturnType operator()(const IterationSummary& summary); + + private: + const MinimizerType minimizer_type; + const bool log_to_stdout_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CALLBACKS_H_ diff --git a/extern/ceres/internal/ceres/casts.h b/extern/ceres/internal/ceres/casts.h new file mode 100644 index 00000000000..f18fdea2d86 --- /dev/null +++ b/extern/ceres/internal/ceres/casts.h @@ -0,0 +1,108 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CASTS_H_ +#define CERES_INTERNAL_CASTS_H_ + +#include <cassert> +#include <cstddef> // For NULL. + +namespace ceres { + +// Identity metafunction. +template <class T> +struct identity_ { + typedef T type; +}; + +// Use implicit_cast as a safe version of static_cast or const_cast +// for implicit conversions. For example: +// - Upcasting in a type hierarchy. +// - Performing arithmetic conversions (int32 to int64, int to double, etc.). +// - Adding const or volatile qualifiers. +// +// In general, implicit_cast can be used to convert this code +// To to = from; +// DoSomething(to); +// to this +// DoSomething(implicit_cast<To>(from)); +// +// base::identity_ is used to make a non-deduced context, which +// forces all callers to explicitly specify the template argument. +template<typename To> +inline To implicit_cast(typename identity_<To>::type to) { + return to; +} + +// This version of implicit_cast is used when two template arguments +// are specified. It's obsolete and should not be used. +template<typename To, typename From> +inline To implicit_cast(typename identity_<From>::type const &f) { + return f; +} + +// When you upcast (that is, cast a pointer from type Foo to type +// SuperclassOfFoo), it's fine to use implicit_cast<>, since upcasts +// always succeed. When you downcast (that is, cast a pointer from +// type Foo to type SubclassOfFoo), static_cast<> isn't safe, because +// how do you know the pointer is really of type SubclassOfFoo? It +// could be a bare Foo, or of type DifferentSubclassOfFoo. Thus, +// when you downcast, you should use this macro. In debug mode, we +// use dynamic_cast<> to double-check the downcast is legal (we die +// if it's not). In normal mode, we do the efficient static_cast<> +// instead. Thus, it's important to test in debug mode to make sure +// the cast is legal! +// This is the only place in the code we should use dynamic_cast<>. +// In particular, you SHOULDN'T be using dynamic_cast<> in order to +// do RTTI (eg code like this: +// if (dynamic_cast<Subclass1>(foo)) HandleASubclass1Object(foo); +// if (dynamic_cast<Subclass2>(foo)) HandleASubclass2Object(foo); +// You should design the code some other way not to need this. + +template<typename To, typename From> // use like this: down_cast<T*>(foo); +inline To down_cast(From* f) { // so we only accept pointers + // Ensures that To is a sub-type of From *. This test is here only + // for compile-time type checking, and has no overhead in an + // optimized build at run-time, as it will be optimized away + // completely. + + // TODO(csilvers): This should use COMPILE_ASSERT. + if (false) { + implicit_cast<From*, To>(NULL); + } + + // uses RTTI in dbg and fastbuild. asserts are disabled in opt builds. + assert(f == NULL || dynamic_cast<To>(f) != NULL); // NOLINT + return static_cast<To>(f); +} + +} // namespace ceres + +#endif // CERES_INTERNAL_CASTS_H_ diff --git a/extern/ceres/internal/ceres/cgnr_linear_operator.h b/extern/ceres/internal/ceres/cgnr_linear_operator.h new file mode 100644 index 00000000000..44c07cabd01 --- /dev/null +++ b/extern/ceres/internal/ceres/cgnr_linear_operator.h @@ -0,0 +1,120 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ +#define CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ + +#include <algorithm> +#include "ceres/linear_operator.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +class SparseMatrix; + +// A linear operator which takes a matrix A and a diagonal vector D and +// performs products of the form +// +// (A^T A + D^T D)x +// +// This is used to implement iterative general sparse linear solving with +// conjugate gradients, where A is the Jacobian and D is a regularizing +// parameter. A brief proof that D^T D is the correct regularizer: +// +// Given a regularized least squares problem: +// +// min ||Ax - b||^2 + ||Dx||^2 +// x +// +// First expand into matrix notation: +// +// (Ax - b)^T (Ax - b) + xD^TDx +// +// Then multiply out to get: +// +// = xA^TAx - 2b^T Ax + b^Tb + xD^TDx +// +// Take the derivative: +// +// 0 = 2A^TAx - 2A^T b + 2 D^TDx +// 0 = A^TAx - A^T b + D^TDx +// 0 = (A^TA + D^TD)x - A^T b +// +// Thus, the symmetric system we need to solve for CGNR is +// +// Sx = z +// +// with S = A^TA + D^TD +// and z = A^T b +// +// Note: This class is not thread safe, since it uses some temporary storage. +class CgnrLinearOperator : public LinearOperator { + public: + CgnrLinearOperator(const LinearOperator& A, const double *D) + : A_(A), D_(D), z_(new double[A.num_rows()]) { + } + virtual ~CgnrLinearOperator() {} + + virtual void RightMultiply(const double* x, double* y) const { + std::fill(z_.get(), z_.get() + A_.num_rows(), 0.0); + + // z = Ax + A_.RightMultiply(x, z_.get()); + + // y = y + Atz + A_.LeftMultiply(z_.get(), y); + + // y = y + DtDx + if (D_ != NULL) { + int n = A_.num_cols(); + VectorRef(y, n).array() += ConstVectorRef(D_, n).array().square() * + ConstVectorRef(x, n).array(); + } + } + + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + + virtual int num_rows() const { return A_.num_cols(); } + virtual int num_cols() const { return A_.num_cols(); } + + private: + const LinearOperator& A_; + const double* D_; + scoped_array<double> z_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ diff --git a/extern/ceres/internal/ceres/cgnr_solver.cc b/extern/ceres/internal/ceres/cgnr_solver.cc new file mode 100644 index 00000000000..61fae758d5b --- /dev/null +++ b/extern/ceres/internal/ceres/cgnr_solver.cc @@ -0,0 +1,88 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/cgnr_solver.h" + +#include "ceres/block_jacobi_preconditioner.h" +#include "ceres/cgnr_linear_operator.h" +#include "ceres/conjugate_gradients_solver.h" +#include "ceres/internal/eigen.h" +#include "ceres/linear_solver.h" +#include "ceres/wall_time.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +CgnrSolver::CgnrSolver(const LinearSolver::Options& options) + : options_(options), + preconditioner_(NULL) { + if (options_.preconditioner_type != JACOBI && + options_.preconditioner_type != IDENTITY) { + LOG(FATAL) << "CGNR only supports IDENTITY and JACOBI preconditioners."; + } +} + +LinearSolver::Summary CgnrSolver::SolveImpl( + BlockSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + EventLogger event_logger("CgnrSolver::Solve"); + + // Form z = Atb. + Vector z(A->num_cols()); + z.setZero(); + A->LeftMultiply(b, z.data()); + + // Precondition if necessary. + LinearSolver::PerSolveOptions cg_per_solve_options = per_solve_options; + if (options_.preconditioner_type == JACOBI) { + if (preconditioner_.get() == NULL) { + preconditioner_.reset(new BlockJacobiPreconditioner(*A)); + } + preconditioner_->Update(*A, per_solve_options.D); + cg_per_solve_options.preconditioner = preconditioner_.get(); + } + + // Solve (AtA + DtD)x = z (= Atb). + VectorRef(x, A->num_cols()).setZero(); + CgnrLinearOperator lhs(*A, per_solve_options.D); + event_logger.AddEvent("Setup"); + + ConjugateGradientsSolver conjugate_gradient_solver(options_); + LinearSolver::Summary summary = + conjugate_gradient_solver.Solve(&lhs, z.data(), cg_per_solve_options, x); + event_logger.AddEvent("Solve"); + return summary; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/cgnr_solver.h b/extern/ceres/internal/ceres/cgnr_solver.h new file mode 100644 index 00000000000..f7a15736925 --- /dev/null +++ b/extern/ceres/internal/ceres/cgnr_solver.h @@ -0,0 +1,69 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CGNR_SOLVER_H_ +#define CERES_INTERNAL_CGNR_SOLVER_H_ + +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_solver.h" + +namespace ceres { +namespace internal { + +class Preconditioner; + +class BlockJacobiPreconditioner; + +// A conjugate gradients on the normal equations solver. This directly solves +// for the solution to +// +// (A^T A + D^T D)x = A^T b +// +// as required for solving for x in the least squares sense. Currently only +// block diagonal preconditioning is supported. +class CgnrSolver : public BlockSparseMatrixSolver { + public: + explicit CgnrSolver(const LinearSolver::Options& options); + virtual Summary SolveImpl( + BlockSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + private: + const LinearSolver::Options options_; + scoped_ptr<Preconditioner> preconditioner_; + CERES_DISALLOW_COPY_AND_ASSIGN(CgnrSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CGNR_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/collections_port.h b/extern/ceres/internal/ceres/collections_port.h new file mode 100644 index 00000000000..e699a661b8b --- /dev/null +++ b/extern/ceres/internal/ceres/collections_port.h @@ -0,0 +1,196 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Portable HashMap and HashSet, and a specialized overload for hashing pairs. + +#ifndef CERES_INTERNAL_COLLECTIONS_PORT_H_ +#define CERES_INTERNAL_COLLECTIONS_PORT_H_ + +#include "ceres/internal/port.h" + +#if defined(CERES_NO_UNORDERED_MAP) +# include <map> +# include <set> +#endif + +#if defined(CERES_TR1_UNORDERED_MAP) +# include <tr1/unordered_map> +# include <tr1/unordered_set> +# define CERES_HASH_NAMESPACE_START namespace std { namespace tr1 { +# define CERES_HASH_NAMESPACE_END } } +#endif + +#if defined(CERES_STD_UNORDERED_MAP) +# include <unordered_map> +# include <unordered_set> +# define CERES_HASH_NAMESPACE_START namespace std { +# define CERES_HASH_NAMESPACE_END } +#endif + +#if defined(CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE) +# include <unordered_map> +# include <unordered_set> +# define CERES_HASH_NAMESPACE_START namespace std { namespace tr1 { +# define CERES_HASH_NAMESPACE_END } } +#endif + +#if !defined(CERES_NO_UNORDERED_MAP) && !defined(CERES_TR1_UNORDERED_MAP) && \ + !defined(CERES_STD_UNORDERED_MAP) && !defined(CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE) // NOLINT +# error One of: CERES_NO_UNORDERED_MAP, CERES_TR1_UNORDERED_MAP,\ + CERES_STD_UNORDERED_MAP, CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE must be defined! // NOLINT +#endif + +#include <utility> +#include "ceres/integral_types.h" + +// Some systems don't have access to unordered_map/unordered_set. In +// that case, substitute the hash map/set with normal map/set. The +// price to pay is slower speed for some operations. +#if defined(CERES_NO_UNORDERED_MAP) + +namespace ceres { +namespace internal { + +template<typename K, typename V> +struct HashMap : map<K, V> {}; + +template<typename K> +struct HashSet : set<K> {}; + +} // namespace internal +} // namespace ceres + +#else + +namespace ceres { +namespace internal { + +#if defined(CERES_TR1_UNORDERED_MAP) || \ + defined(CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE) +template<typename K, typename V> +struct HashMap : std::tr1::unordered_map<K, V> {}; +template<typename K> +struct HashSet : std::tr1::unordered_set<K> {}; +#endif + +#if defined(CERES_STD_UNORDERED_MAP) +template<typename K, typename V> +struct HashMap : std::unordered_map<K, V> {}; +template<typename K> +struct HashSet : std::unordered_set<K> {}; +#endif + +#if defined(_WIN32) && !defined(__MINGW64__) && !defined(__MINGW32__) +#define GG_LONGLONG(x) x##I64 +#define GG_ULONGLONG(x) x##UI64 +#else +#define GG_LONGLONG(x) x##LL +#define GG_ULONGLONG(x) x##ULL +#endif + +// The hash function is due to Bob Jenkins (see +// http://burtleburtle.net/bob/hash/index.html). Each mix takes 36 instructions, +// in 18 cycles if you're lucky. On x86 architectures, this requires 45 +// instructions in 27 cycles, if you're lucky. +// +// 32bit version +inline void hash_mix(uint32& a, uint32& b, uint32& c) { + a -= b; a -= c; a ^= (c>>13); + b -= c; b -= a; b ^= (a<<8); + c -= a; c -= b; c ^= (b>>13); + a -= b; a -= c; a ^= (c>>12); + b -= c; b -= a; b ^= (a<<16); + c -= a; c -= b; c ^= (b>>5); + a -= b; a -= c; a ^= (c>>3); + b -= c; b -= a; b ^= (a<<10); + c -= a; c -= b; c ^= (b>>15); +} + +// 64bit version +inline void hash_mix(uint64& a, uint64& b, uint64& c) { + a -= b; a -= c; a ^= (c>>43); + b -= c; b -= a; b ^= (a<<9); + c -= a; c -= b; c ^= (b>>8); + a -= b; a -= c; a ^= (c>>38); + b -= c; b -= a; b ^= (a<<23); + c -= a; c -= b; c ^= (b>>5); + a -= b; a -= c; a ^= (c>>35); + b -= c; b -= a; b ^= (a<<49); + c -= a; c -= b; c ^= (b>>11); +} + +inline uint32 Hash32NumWithSeed(uint32 num, uint32 c) { + // The golden ratio; an arbitrary value. + uint32 b = 0x9e3779b9UL; + hash_mix(num, b, c); + return c; +} + +inline uint64 Hash64NumWithSeed(uint64 num, uint64 c) { + // More of the golden ratio. + uint64 b = GG_ULONGLONG(0xe08c1d668b756f82); + hash_mix(num, b, c); + return c; +} + +} // namespace internal +} // namespace ceres + +// Since on some platforms this is a doubly-nested namespace (std::tr1) and +// others it is not, the entire namespace line must be in a macro. +CERES_HASH_NAMESPACE_START + +// The outrageously annoying specializations below are for portability reasons. +// In short, it's not possible to have two overloads of hash<pair<T1, T2> + +// Hasher for STL pairs. Requires hashers for both members to be defined. +template<typename T> +struct hash<pair<T, T> > { + size_t operator()(const pair<T, T>& p) const { + size_t h1 = hash<T>()(p.first); + size_t h2 = hash<T>()(p.second); + // The decision below is at compile time + return (sizeof(h1) <= sizeof(ceres::internal::uint32)) ? + ceres::internal::Hash32NumWithSeed(h1, h2) : + ceres::internal::Hash64NumWithSeed(h1, h2); + } + // Less than operator for MSVC. + bool operator()(const pair<T, T>& a, + const pair<T, T>& b) const { + return a < b; + } + static const size_t bucket_size = 4; // These are required by MSVC + static const size_t min_buckets = 8; // 4 and 8 are defaults. +}; + +CERES_HASH_NAMESPACE_END + +#endif // CERES_NO_UNORDERED_MAP +#endif // CERES_INTERNAL_COLLECTIONS_PORT_H_ diff --git a/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc b/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc new file mode 100644 index 00000000000..ebb2a62c544 --- /dev/null +++ b/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc @@ -0,0 +1,122 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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 { + +using std::vector; + +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 = + std::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/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h b/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h new file mode 100644 index 00000000000..da2109fba3e --- /dev/null +++ b/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h @@ -0,0 +1,144 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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 std::vector<int>& row_blocks, + const std::vector<int>& col_blocks, + std::vector<int>* block_rows, + std::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 std::vector<int>& blocks, + const std::vector<int>& block_ordering, + std::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) { + std::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/ceres/internal/ceres/compressed_row_jacobian_writer.cc b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc new file mode 100644 index 00000000000..64b6ac00447 --- /dev/null +++ b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc @@ -0,0 +1,233 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/compressed_row_jacobian_writer.h" + +#include <utility> +#include <vector> + +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/scratch_evaluate_preparer.h" + +namespace ceres { +namespace internal { + +using std::make_pair; +using std::pair; +using std::vector; + +void CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors( + const Program* program, CompressedRowSparseMatrix* jacobian) { + const vector<ParameterBlock*>& parameter_blocks = + program->parameter_blocks(); + vector<int>& col_blocks = *(jacobian->mutable_col_blocks()); + col_blocks.resize(parameter_blocks.size()); + for (int i = 0; i < parameter_blocks.size(); ++i) { + col_blocks[i] = parameter_blocks[i]->LocalSize(); + } + + const vector<ResidualBlock*>& residual_blocks = + program->residual_blocks(); + vector<int>& row_blocks = *(jacobian->mutable_row_blocks()); + row_blocks.resize(residual_blocks.size()); + for (int i = 0; i < residual_blocks.size(); ++i) { + row_blocks[i] = residual_blocks[i]->NumResiduals(); + } +} + +void CompressedRowJacobianWriter::GetOrderedParameterBlocks( + const Program* program, + int residual_id, + vector<pair<int, int> >* evaluated_jacobian_blocks) { + const ResidualBlock* residual_block = + program->residual_blocks()[residual_id]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + for (int j = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + evaluated_jacobian_blocks->push_back( + make_pair(parameter_block->index(), j)); + } + } + sort(evaluated_jacobian_blocks->begin(), evaluated_jacobian_blocks->end()); +} + +SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { + const vector<ResidualBlock*>& residual_blocks = + program_->residual_blocks(); + + int total_num_residuals = program_->NumResiduals(); + int total_num_effective_parameters = program_->NumEffectiveParameters(); + + // Count the number of jacobian nonzeros. + int num_jacobian_nonzeros = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + num_jacobian_nonzeros += num_residuals * parameter_block->LocalSize(); + } + } + } + + // Allocate storage for the jacobian with some extra space at the end. + // Allocate more space than needed to store the jacobian so that when the LM + // algorithm adds the diagonal, no reallocation is necessary. This reduces + // peak memory usage significantly. + CompressedRowSparseMatrix* jacobian = + new CompressedRowSparseMatrix( + total_num_residuals, + total_num_effective_parameters, + num_jacobian_nonzeros + total_num_effective_parameters); + + // At this stage, the CompressedRowSparseMatrix is an invalid state. But this + // seems to be the only way to construct it without doing a memory copy. + int* rows = jacobian->mutable_rows(); + int* cols = jacobian->mutable_cols(); + int row_pos = 0; + rows[0] = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Count the number of derivatives for a row of this residual block and + // build a list of active parameter block indices. + int num_derivatives = 0; + vector<int> parameter_indices; + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + parameter_indices.push_back(parameter_block->index()); + num_derivatives += parameter_block->LocalSize(); + } + } + + // Sort the parameters by their position in the state vector. + sort(parameter_indices.begin(), parameter_indices.end()); + CHECK(unique(parameter_indices.begin(), parameter_indices.end()) == + parameter_indices.end()) + << "Ceres internal error: " + << "Duplicate parameter blocks detected in a cost function. " + << "This should never happen. Please report this to " + << "the Ceres developers."; + + // Update the row indices. + const int num_residuals = residual_block->NumResiduals(); + for (int j = 0; j < num_residuals; ++j) { + rows[row_pos + j + 1] = rows[row_pos + j] + num_derivatives; + } + + // Iterate over parameter blocks in the order which they occur in the + // parameter vector. This code mirrors that in Write(), where jacobian + // values are updated. + int col_pos = 0; + for (int j = 0; j < parameter_indices.size(); ++j) { + ParameterBlock* parameter_block = + program_->parameter_blocks()[parameter_indices[j]]; + const int parameter_block_size = parameter_block->LocalSize(); + + for (int r = 0; r < num_residuals; ++r) { + // This is the position in the values array of the jacobian where this + // row of the jacobian block should go. + const int column_block_begin = rows[row_pos + r] + col_pos; + + for (int c = 0; c < parameter_block_size; ++c) { + cols[column_block_begin + c] = parameter_block->delta_offset() + c; + } + } + col_pos += parameter_block_size; + } + row_pos += num_residuals; + } + CHECK_EQ(num_jacobian_nonzeros, rows[total_num_residuals]); + + PopulateJacobianRowAndColumnBlockVectors(program_, jacobian); + + return jacobian; +} + +void CompressedRowJacobianWriter::Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian) { + CompressedRowSparseMatrix* jacobian = + down_cast<CompressedRowSparseMatrix*>(base_jacobian); + + double* jacobian_values = jacobian->mutable_values(); + const int* jacobian_rows = jacobian->rows(); + + const ResidualBlock* residual_block = + program_->residual_blocks()[residual_id]; + const int num_residuals = residual_block->NumResiduals(); + + vector<pair<int, int> > evaluated_jacobian_blocks; + GetOrderedParameterBlocks(program_, residual_id, &evaluated_jacobian_blocks); + + // Where in the current row does the jacobian for a parameter block begin. + int col_pos = 0; + + // Iterate over the jacobian blocks in increasing order of their + // positions in the reduced parameter vector. + for (int i = 0; i < evaluated_jacobian_blocks.size(); ++i) { + const ParameterBlock* parameter_block = + program_->parameter_blocks()[evaluated_jacobian_blocks[i].first]; + const int argument = evaluated_jacobian_blocks[i].second; + const int parameter_block_size = parameter_block->LocalSize(); + + // Copy one row of the jacobian block at a time. + for (int r = 0; r < num_residuals; ++r) { + // Position of the r^th row of the current jacobian block. + const double* block_row_begin = + jacobians[argument] + r * parameter_block_size; + + // Position in the values array of the jacobian where this + // row of the jacobian block should go. + double* column_block_begin = + jacobian_values + jacobian_rows[residual_offset + r] + col_pos; + + std::copy(block_row_begin, + block_row_begin + parameter_block_size, + column_block_begin); + } + col_pos += parameter_block_size; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/compressed_row_jacobian_writer.h b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.h new file mode 100644 index 00000000000..1cd01235ccf --- /dev/null +++ b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.h @@ -0,0 +1,112 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A jacobian writer that directly writes to compressed row sparse matrices. + +#ifndef CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ + +#include <utility> +#include <vector> + +#include "ceres/evaluator.h" +#include "ceres/scratch_evaluate_preparer.h" + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; +class Program; +class SparseMatrix; + +class CompressedRowJacobianWriter { + public: + CompressedRowJacobianWriter(Evaluator::Options /* ignored */, + Program* program) + : program_(program) { + } + + // PopulateJacobianRowAndColumnBlockVectors sets col_blocks and + // row_blocks for a CompressedRowSparseMatrix, based on the + // parameter block sizes and residual sizes respectively from the + // program. This is useful when Solver::Options::use_block_amd = + // true; + // + // This function is static so that it is available to other jacobian + // writers which use CompressedRowSparseMatrix (or derived types). + // (Jacobian writers do not fall under any type hierarchy; they only + // have to provide an interface as specified in program_evaluator.h). + static void PopulateJacobianRowAndColumnBlockVectors( + const Program* program, + CompressedRowSparseMatrix* jacobian); + + // It is necessary to determine the order of the jacobian blocks + // before copying them into a CompressedRowSparseMatrix (or derived + // type). Just because a cost function uses parameter blocks 1 + // after 2 in its arguments does not mean that the block 1 occurs + // before block 2 in the column layout of the jacobian. Thus, + // GetOrderedParameterBlocks determines the order by sorting the + // jacobian blocks by their position in the state vector. + // + // This function is static so that it is available to other jacobian + // writers which use CompressedRowSparseMatrix (or derived types). + // (Jacobian writers do not fall under any type hierarchy; they only + // have to provide an interface as specified in + // program_evaluator.h). + static void GetOrderedParameterBlocks( + const Program* program, + int residual_id, + std::vector<std::pair<int, int> >* evaluated_jacobian_blocks); + + // JacobianWriter interface. + + // Since the compressed row matrix has different layout than that + // assumed by the cost functions, use scratch space to store the + // jacobians temporarily then copy them over to the larger jacobian + // in the Write() function. + ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads) { + return ScratchEvaluatePreparer::Create(*program_, num_threads); + } + + SparseMatrix* CreateJacobian() const; + + void Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian); + + private: + Program* program_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ diff --git a/extern/ceres/internal/ceres/compressed_row_sparse_matrix.cc b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.cc new file mode 100644 index 00000000000..91d18bbd604 --- /dev/null +++ b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.cc @@ -0,0 +1,562 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/compressed_row_sparse_matrix.h" + +#include <algorithm> +#include <numeric> +#include <vector> +#include "ceres/crs_matrix.h" +#include "ceres/internal/port.h" +#include "ceres/triplet_sparse_matrix.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::vector; + +namespace { + +// Helper functor used by the constructor for reordering the contents +// of a TripletSparseMatrix. This comparator assumes thay there are no +// duplicates in the pair of arrays rows and cols, i.e., there is no +// indices i and j (not equal to each other) s.t. +// +// rows[i] == rows[j] && cols[i] == cols[j] +// +// If this is the case, this functor will not be a StrictWeakOrdering. +struct RowColLessThan { + RowColLessThan(const int* rows, const int* cols) + : rows(rows), cols(cols) { + } + + bool operator()(const int x, const int y) const { + if (rows[x] == rows[y]) { + return (cols[x] < cols[y]); + } + return (rows[x] < rows[y]); + } + + const int* rows; + const int* cols; +}; + +} // namespace + +// This constructor gives you a semi-initialized CompressedRowSparseMatrix. +CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros) { + num_rows_ = num_rows; + num_cols_ = num_cols; + 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: " << 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(); + + 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); + for (int i = 0; i < m.num_nonzeros(); ++i) { + index[i] = i; + } + + // Sort index such that the entries of m are ordered by row and ties + // are broken by column. + sort(index.begin(), index.end(), RowColLessThan(m.rows(), m.cols())); + + VLOG(1) << "# of rows: " << num_rows_ + << " # of columns: " << num_cols_ + << " max_num_nonzeros: " << 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. + for (int i = 0; i < m.num_nonzeros(); ++i) { + const int idx = index[i]; + ++rows_[m.rows()[idx] + 1]; + cols_[i] = m.cols()[idx]; + values_[i] = m.values()[idx]; + } + + // Find the cumulative sum of the row counts. + for (int i = 1; i < num_rows_ + 1; ++i) { + rows_[i] += rows_[i - 1]; + } + + CHECK_EQ(num_nonzeros(), m.num_nonzeros()); +} + +CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, + int num_rows) { + CHECK_NOTNULL(diagonal); + + num_rows_ = num_rows; + num_cols_ = 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) { + cols_[i] = i; + values_[i] = diagonal[i]; + rows_[i + 1] = i + 1; + } + + CHECK_EQ(num_nonzeros(), num_rows); +} + +CompressedRowSparseMatrix::~CompressedRowSparseMatrix() { +} + +void CompressedRowSparseMatrix::SetZero() { + std::fill(values_.begin(), values_.end(), 0); +} + +void CompressedRowSparseMatrix::RightMultiply(const double* x, + double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + y[r] += values_[idx] * x[cols_[idx]]; + } + } +} + +void CompressedRowSparseMatrix::LeftMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + y[cols_[idx]] += values_[idx] * x[r]; + } + } +} + +void CompressedRowSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + + std::fill(x, x + num_cols_, 0.0); + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + x[cols_[idx]] += values_[idx] * values_[idx]; + } +} + +void CompressedRowSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + values_[idx] *= scale[cols_[idx]]; + } +} + +void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + CHECK_NOTNULL(dense_matrix); + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + (*dense_matrix)(r, cols_[idx]) = values_[idx]; + } + } +} + +void CompressedRowSparseMatrix::DeleteRows(int delta_rows) { + CHECK_GE(delta_rows, 0); + CHECK_LE(delta_rows, num_rows_); + + num_rows_ -= delta_rows; + rows_.resize(num_rows_ + 1); + + // Walk the list of row blocks until we reach the new number of rows + // and the drop the rest of the row blocks. + int num_row_blocks = 0; + int num_rows = 0; + while (num_row_blocks < row_blocks_.size() && num_rows < num_rows_) { + num_rows += row_blocks_[num_row_blocks]; + ++num_row_blocks; + } + + row_blocks_.resize(num_row_blocks); +} + +void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { + CHECK_EQ(m.num_cols(), num_cols_); + + CHECK(row_blocks_.size() == 0 || m.row_blocks().size() !=0) + << "Cannot append a matrix with row blocks to one without and vice versa." + << "This matrix has : " << row_blocks_.size() << " row blocks." + << "The matrix being appended has: " << m.row_blocks().size() + << " row blocks."; + + if (m.num_rows() == 0) { + return; + } + + 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. + DCHECK_LT(num_nonzeros(), cols_.size()); + if (m.num_nonzeros() > 0) { + std::copy(m.cols(), m.cols() + m.num_nonzeros(), &cols_[num_nonzeros()]); + std::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_]] + std::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) { + rows_[num_rows_ + r] += m.rows()[r]; + } + + num_rows_ += m.num_rows(); + row_blocks_.insert(row_blocks_.end(), + m.row_blocks().begin(), + m.row_blocks().end()); +} + +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]); + } + } +} + +void CompressedRowSparseMatrix::ToCRSMatrix(CRSMatrix* matrix) const { + 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(matrix->rows[matrix->num_rows]); + matrix->values.resize(matrix->rows[matrix->num_rows]); +} + +void CompressedRowSparseMatrix::SetMaxNumNonZeros(int num_nonzeros) { + CHECK_GE(num_nonzeros, 0); + + cols_.resize(num_nonzeros); + values_.resize(num_nonzeros); +} + +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(); + std::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; + + *(transpose->mutable_row_blocks()) = col_blocks_; + *(transpose->mutable_col_blocks()) = row_blocks_; + + return transpose; +} + +namespace { +// A ProductTerm is a term in the outer product of a matrix with +// itself. +struct ProductTerm { + ProductTerm(const int row, const int col, const int index) + : row(row), col(col), index(index) { + } + + bool operator<(const ProductTerm& right) const { + if (row == right.row) { + if (col == right.col) { + return index < right.index; + } + return col < right.col; + } + return row < right.row; + } + + int row; + int col; + int index; +}; + +CompressedRowSparseMatrix* +CompressAndFillProgram(const int num_rows, + const int num_cols, + const vector<ProductTerm>& product, + vector<int>* program) { + CHECK_GT(product.size(), 0); + + // Count the number of unique product term, which in turn is the + // number of non-zeros in the outer product. + int num_nonzeros = 1; + for (int i = 1; i < product.size(); ++i) { + if (product[i].row != product[i - 1].row || + product[i].col != product[i - 1].col) { + ++num_nonzeros; + } + } + + CompressedRowSparseMatrix* matrix = + new CompressedRowSparseMatrix(num_rows, num_cols, num_nonzeros); + + int* crsm_rows = matrix->mutable_rows(); + std::fill(crsm_rows, crsm_rows + num_rows + 1, 0); + int* crsm_cols = matrix->mutable_cols(); + std::fill(crsm_cols, crsm_cols + num_nonzeros, 0); + + CHECK_NOTNULL(program)->clear(); + program->resize(product.size()); + + // Iterate over the sorted product terms. This means each row is + // filled one at a time, and we are able to assign a position in the + // values array to each term. + // + // If terms repeat, i.e., they contribute to the same entry in the + // result matrix), then they do not affect the sparsity structure of + // the result matrix. + int nnz = 0; + crsm_cols[0] = product[0].col; + crsm_rows[product[0].row + 1]++; + (*program)[product[0].index] = nnz; + for (int i = 1; i < product.size(); ++i) { + const ProductTerm& previous = product[i - 1]; + const ProductTerm& current = product[i]; + + // Sparsity structure is updated only if the term is not a repeat. + if (previous.row != current.row || previous.col != current.col) { + crsm_cols[++nnz] = current.col; + crsm_rows[current.row + 1]++; + } + + // All terms get assigned the position in the values array where + // their value is accumulated. + (*program)[current.index] = nnz; + } + + for (int i = 1; i < num_rows + 1; ++i) { + crsm_rows[i] += crsm_rows[i - 1]; + } + + return matrix; +} + +} // namespace + +CompressedRowSparseMatrix* +CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( + const CompressedRowSparseMatrix& m, + vector<int>* program) { + CHECK_NOTNULL(program)->clear(); + CHECK_GT(m.num_nonzeros(), 0) + << "Congratulations, " + << "you found a bug in Ceres. Please report it."; + + vector<ProductTerm> product; + const vector<int>& row_blocks = m.row_blocks(); + int row_block_begin = 0; + // Iterate over row blocks + for (int row_block = 0; row_block < row_blocks.size(); ++row_block) { + const int row_block_end = row_block_begin + row_blocks[row_block]; + // Compute the outer product terms for just one row per row block. + const int r = row_block_begin; + // Compute the lower triangular part of the product. + for (int idx1 = m.rows()[r]; idx1 < m.rows()[r + 1]; ++idx1) { + for (int idx2 = m.rows()[r]; idx2 <= idx1; ++idx2) { + product.push_back(ProductTerm(m.cols()[idx1], + m.cols()[idx2], + product.size())); + } + } + row_block_begin = row_block_end; + } + CHECK_EQ(row_block_begin, m.num_rows()); + sort(product.begin(), product.end()); + return CompressAndFillProgram(m.num_cols(), m.num_cols(), product, program); +} + +void CompressedRowSparseMatrix::ComputeOuterProduct( + const CompressedRowSparseMatrix& m, + const vector<int>& program, + CompressedRowSparseMatrix* result) { + result->SetZero(); + double* values = result->mutable_values(); + const vector<int>& row_blocks = m.row_blocks(); + + int cursor = 0; + int row_block_begin = 0; + const double* m_values = m.values(); + const int* m_rows = m.rows(); + // Iterate over row blocks. + for (int row_block = 0; row_block < row_blocks.size(); ++row_block) { + const int row_block_end = row_block_begin + row_blocks[row_block]; + const int saved_cursor = cursor; + for (int r = row_block_begin; r < row_block_end; ++r) { + // Reuse the program segment for each row in this row block. + cursor = saved_cursor; + const int row_begin = m_rows[r]; + const int row_end = m_rows[r + 1]; + for (int idx1 = row_begin; idx1 < row_end; ++idx1) { + const double v1 = m_values[idx1]; + for (int idx2 = row_begin; idx2 <= idx1; ++idx2, ++cursor) { + values[program[cursor]] += v1 * m_values[idx2]; + } + } + } + row_block_begin = row_block_end; + } + + CHECK_EQ(row_block_begin, m.num_rows()); + CHECK_EQ(cursor, program.size()); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/compressed_row_sparse_matrix.h b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.h new file mode 100644 index 00000000000..987339d09a1 --- /dev/null +++ b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.h @@ -0,0 +1,181 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ + +#include <vector> +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/sparse_matrix.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +struct CRSMatrix; + +namespace internal { + +class TripletSparseMatrix; + +class CompressedRowSparseMatrix : public SparseMatrix { + public: + // Build a matrix with the same content as the TripletSparseMatrix + // m. TripletSparseMatrix objects are easier to construct + // incrementally, so we use them to initialize SparseMatrix + // objects. + // + // We assume that m does not have any repeated entries. + explicit CompressedRowSparseMatrix(const TripletSparseMatrix& m); + + // Use this constructor only if you know what you are doing. This + // creates a "blank" matrix with the appropriate amount of memory + // allocated. However, the object itself is in an inconsistent state + // as the rows and cols matrices do not match the values of + // num_rows, num_cols and max_num_nonzeros. + // + // The use case for this constructor is that when the user knows the + // size of the matrix to begin with and wants to update the layout + // manually, instead of going via the indirect route of first + // constructing a TripletSparseMatrix, which leads to more than + // double the peak memory usage. + CompressedRowSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros); + + // Build a square sparse diagonal matrix with num_rows rows and + // columns. The diagonal m(i,i) = diagonal(i); + CompressedRowSparseMatrix(const double* diagonal, int num_rows); + + virtual ~CompressedRowSparseMatrix(); + + // SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + + virtual void ToDenseMatrix(Matrix* dense_matrix) const; + 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_[0]; } + virtual double* mutable_values() { return &values_[0]; } + + // Delete the bottom delta_rows. + // num_rows -= delta_rows + void DeleteRows(int delta_rows); + + // Append the contents of m to the bottom of this matrix. m must + // have the same number of columns as this matrix. + void AppendRows(const CompressedRowSparseMatrix& m); + + void ToCRSMatrix(CRSMatrix* matrix) const; + + // Low level access methods that expose the structure of the matrix. + const int* cols() const { return &cols_[0]; } + int* mutable_cols() { return &cols_[0]; } + + const int* rows() const { return &rows_[0]; } + int* mutable_rows() { return &rows_[0]; } + + const std::vector<int>& row_blocks() const { return row_blocks_; } + std::vector<int>* mutable_row_blocks() { return &row_blocks_; } + + const std::vector<int>& col_blocks() const { return col_blocks_; } + std::vector<int>* mutable_col_blocks() { return &col_blocks_; } + + // Destructive array resizing method. + void SetMaxNumNonZeros(int num_nonzeros); + + // 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 std::vector<int>& blocks); + + // Compute the sparsity structure of the product m.transpose() * m + // and create a CompressedRowSparseMatrix corresponding to it. + // + // Also compute a "program" vector, which for every term in the + // outer product points to the entry in the values array of the + // result matrix where it should be accumulated. + // + // This program is used by the ComputeOuterProduct function below to + // compute the outer product. + // + // Since the entries of the program are the same for rows with the + // same sparsity structure, the program only stores the result for + // one row per row block. The ComputeOuterProduct function reuses + // this information for each row in the row block. + static CompressedRowSparseMatrix* CreateOuterProductMatrixAndProgram( + const CompressedRowSparseMatrix& m, + std::vector<int>* program); + + // Compute the values array for the expression m.transpose() * m, + // where the matrix used to store the result and a program have been + // created using the CreateOuterProductMatrixAndProgram function + // above. + static void ComputeOuterProduct(const CompressedRowSparseMatrix& m, + const std::vector<int>& program, + CompressedRowSparseMatrix* result); + + private: + int num_rows_; + int num_cols_; + std::vector<int> rows_; + std::vector<int> cols_; + std::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 + // optional information for use by algorithms operating on the + // matrix. The class itself does not make use of this information in + // any way. + std::vector<int> row_blocks_; + std::vector<int> col_blocks_; + + CERES_DISALLOW_COPY_AND_ASSIGN(CompressedRowSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/conditioned_cost_function.cc b/extern/ceres/internal/ceres/conditioned_cost_function.cc new file mode 100644 index 00000000000..08899e3d246 --- /dev/null +++ b/extern/ceres/internal/ceres/conditioned_cost_function.cc @@ -0,0 +1,130 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: wjr@google.com (William Rucklidge) +// +// This file contains the implementation of the conditioned cost function. + +#include "ceres/conditioned_cost_function.h" + +#include <cstddef> + +#include "ceres/internal/eigen.h" +#include "ceres/stl_util.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +// This cost function has the same dimensions (parameters, residuals) as +// the one it's wrapping. +ConditionedCostFunction::ConditionedCostFunction( + CostFunction* wrapped_cost_function, + const std::vector<CostFunction*>& conditioners, + Ownership ownership) + : wrapped_cost_function_(wrapped_cost_function), + conditioners_(conditioners), + ownership_(ownership) { + // Set up our dimensions. + set_num_residuals(wrapped_cost_function_->num_residuals()); + *mutable_parameter_block_sizes() = + wrapped_cost_function_->parameter_block_sizes(); + + // Sanity-check the conditioners' dimensions. + CHECK_EQ(wrapped_cost_function_->num_residuals(), conditioners_.size()); + for (int i = 0; i < wrapped_cost_function_->num_residuals(); i++) { + if (conditioners[i]) { + CHECK_EQ(1, conditioners[i]->num_residuals()); + CHECK_EQ(1, conditioners[i]->parameter_block_sizes().size()); + CHECK_EQ(1, conditioners[i]->parameter_block_sizes()[0]); + } + } +} + +ConditionedCostFunction::~ConditionedCostFunction() { + if (ownership_ == TAKE_OWNERSHIP) { + STLDeleteElements(&conditioners_); + } else { + wrapped_cost_function_.release(); + } +} + +bool ConditionedCostFunction::Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + bool success = wrapped_cost_function_->Evaluate(parameters, residuals, + jacobians); + if (!success) { + return false; + } + + for (int r = 0; r < wrapped_cost_function_->num_residuals(); r++) { + // On output, we want to have + // residuals[r] = conditioners[r](wrapped_residuals[r]) + // For parameter block i, column c, + // jacobians[i][r*parameter_block_size_[i] + c] = + // = d residual[r] / d parameters[i][c] + // = conditioners[r]'(wrapped_residuals[r]) * + // d wrapped_residuals[r] / d parameters[i][c] + if (conditioners_[r]) { + double conditioner_derivative; + double* conditioner_derivative_pointer = &conditioner_derivative; + double** conditioner_derivative_pointer2 = + &conditioner_derivative_pointer; + if (!jacobians) { + conditioner_derivative_pointer2 = NULL; + } + + double unconditioned_residual = residuals[r]; + double* parameter_pointer = &unconditioned_residual; + success = conditioners_[r]->Evaluate(¶meter_pointer, + &residuals[r], + conditioner_derivative_pointer2); + if (!success) { + return false; + } + + if (jacobians) { + for (int i = 0; + i < wrapped_cost_function_->parameter_block_sizes().size(); + i++) { + if (jacobians[i]) { + int parameter_block_size = + wrapped_cost_function_->parameter_block_sizes()[i]; + VectorRef jacobian_row(jacobians[i] + r * parameter_block_size, + parameter_block_size, 1); + jacobian_row *= conditioner_derivative; + } + } + } + } + } + return true; +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/conjugate_gradients_solver.cc b/extern/ceres/internal/ceres/conjugate_gradients_solver.cc new file mode 100644 index 00000000000..3702276a2fb --- /dev/null +++ b/extern/ceres/internal/ceres/conjugate_gradients_solver.cc @@ -0,0 +1,248 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A preconditioned conjugate gradients solver +// (ConjugateGradientsSolver) for positive semidefinite linear +// systems. +// +// We have also augmented the termination criterion used by this +// solver to support not just residual based termination but also +// termination based on decrease in the value of the quadratic model +// that CG optimizes. + +#include "ceres/conjugate_gradients_solver.h" + +#include <cmath> +#include <cstddef> +#include "ceres/fpclassify.h" +#include "ceres/internal/eigen.h" +#include "ceres/linear_operator.h" +#include "ceres/stringprintf.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { + +bool IsZeroOrInfinity(double x) { + return ((x == 0.0) || (IsInfinite(x))); +} + +} // namespace + +ConjugateGradientsSolver::ConjugateGradientsSolver( + const LinearSolver::Options& options) + : options_(options) { +} + +LinearSolver::Summary ConjugateGradientsSolver::Solve( + LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(x); + CHECK_NOTNULL(b); + CHECK_EQ(A->num_rows(), A->num_cols()); + + LinearSolver::Summary summary; + summary.termination_type = LINEAR_SOLVER_NO_CONVERGENCE; + summary.message = "Maximum number of iterations reached."; + summary.num_iterations = 0; + + const int num_cols = A->num_cols(); + VectorRef xref(x, num_cols); + ConstVectorRef bref(b, num_cols); + + const double norm_b = bref.norm(); + if (norm_b == 0.0) { + xref.setZero(); + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Convergence. |b| = 0."; + return summary; + } + + Vector r(num_cols); + Vector p(num_cols); + Vector z(num_cols); + Vector tmp(num_cols); + + const double tol_r = per_solve_options.r_tolerance * norm_b; + + tmp.setZero(); + A->RightMultiply(x, tmp.data()); + r = bref - tmp; + double norm_r = r.norm(); + if (options_.min_num_iterations == 0 && norm_r <= tol_r) { + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = + StringPrintf("Convergence. |r| = %e <= %e.", norm_r, tol_r); + return summary; + } + + double rho = 1.0; + + // Initial value of the quadratic model Q = x'Ax - 2 * b'x. + double Q0 = -1.0 * xref.dot(bref + r); + + for (summary.num_iterations = 1;; ++summary.num_iterations) { + // Apply preconditioner + if (per_solve_options.preconditioner != NULL) { + z.setZero(); + per_solve_options.preconditioner->RightMultiply(r.data(), z.data()); + } else { + z = r; + } + + double last_rho = rho; + rho = r.dot(z); + if (IsZeroOrInfinity(rho)) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = StringPrintf("Numerical failure. rho = r'z = %e.", rho); + break; + } + + if (summary.num_iterations == 1) { + p = z; + } else { + double beta = rho / last_rho; + if (IsZeroOrInfinity(beta)) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = StringPrintf( + "Numerical failure. beta = rho_n / rho_{n-1} = %e, " + "rho_n = %e, rho_{n-1} = %e", beta, rho, last_rho); + break; + } + p = z + beta * p; + } + + Vector& q = z; + q.setZero(); + A->RightMultiply(p.data(), q.data()); + const double pq = p.dot(q); + if ((pq <= 0) || IsInfinite(pq)) { + summary.termination_type = LINEAR_SOLVER_NO_CONVERGENCE; + summary.message = StringPrintf( + "Matrix is indefinite, no more progress can be made. " + "p'q = %e. |p| = %e, |q| = %e", + pq, p.norm(), q.norm()); + break; + } + + const double alpha = rho / pq; + if (IsInfinite(alpha)) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = + StringPrintf("Numerical failure. alpha = rho / pq = %e, " + "rho = %e, pq = %e.", alpha, rho, pq); + break; + } + + xref = xref + alpha * p; + + // Ideally we would just use the update r = r - alpha*q to keep + // track of the residual vector. However this estimate tends to + // drift over time due to round off errors. Thus every + // residual_reset_period iterations, we calculate the residual as + // r = b - Ax. We do not do this every iteration because this + // requires an additional matrix vector multiply which would + // double the complexity of the CG algorithm. + if (summary.num_iterations % options_.residual_reset_period == 0) { + tmp.setZero(); + A->RightMultiply(x, tmp.data()); + r = bref - tmp; + } else { + r = r - alpha * q; + } + + // Quadratic model based termination. + // Q1 = x'Ax - 2 * b' x. + const double Q1 = -1.0 * xref.dot(bref + r); + + // For PSD matrices A, let + // + // Q(x) = x'Ax - 2b'x + // + // be the cost of the quadratic function defined by A and b. Then, + // the solver terminates at iteration i if + // + // i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance. + // + // This termination criterion is more useful when using CG to + // solve the Newton step. This particular convergence test comes + // from Stephen Nash's work on truncated Newton + // methods. References: + // + // 1. Stephen G. Nash & Ariela Sofer, Assessing A Search + // Direction Within A Truncated Newton Method, Operation + // Research Letters 9(1990) 219-221. + // + // 2. Stephen G. Nash, A Survey of Truncated Newton Methods, + // Journal of Computational and Applied Mathematics, + // 124(1-2), 45-59, 2000. + // + const double zeta = summary.num_iterations * (Q1 - Q0) / Q1; + if (zeta < per_solve_options.q_tolerance && + summary.num_iterations >= options_.min_num_iterations) { + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = + StringPrintf("Iteration: %d Convergence: zeta = %e < %e. |r| = %e", + summary.num_iterations, + zeta, + per_solve_options.q_tolerance, + r.norm()); + break; + } + Q0 = Q1; + + // Residual based termination. + norm_r = r. norm(); + if (norm_r <= tol_r && + summary.num_iterations >= options_.min_num_iterations) { + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = + StringPrintf("Iteration: %d Convergence. |r| = %e <= %e.", + summary.num_iterations, + norm_r, + tol_r); + break; + } + + if (summary.num_iterations >= options_.max_num_iterations) { + break; + } + } + + return summary; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/conjugate_gradients_solver.h b/extern/ceres/internal/ceres/conjugate_gradients_solver.h new file mode 100644 index 00000000000..a1e18334414 --- /dev/null +++ b/extern/ceres/internal/ceres/conjugate_gradients_solver.h @@ -0,0 +1,74 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Preconditioned Conjugate Gradients based solver for positive +// semidefinite linear systems. + +#ifndef CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ +#define CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class LinearOperator; + +// This class implements the now classical Conjugate Gradients +// algorithm of Hestenes & Stiefel for solving postive semidefinite +// linear sytems. Optionally it can use a preconditioner also to +// reduce the condition number of the linear system and improve the +// convergence rate. Modern references for Conjugate Gradients are the +// books by Yousef Saad and Trefethen & Bau. This implementation of CG +// has been augmented with additional termination tests that are +// needed for forcing early termination when used as part of an +// inexact Newton solver. +// +// For more details see the documentation for +// LinearSolver::PerSolveOptions::r_tolerance and +// LinearSolver::PerSolveOptions::q_tolerance in linear_solver.h. +class ConjugateGradientsSolver : public LinearSolver { + public: + explicit ConjugateGradientsSolver(const LinearSolver::Options& options); + virtual Summary Solve(LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + private: + const LinearSolver::Options options_; + CERES_DISALLOW_COPY_AND_ASSIGN(ConjugateGradientsSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/coordinate_descent_minimizer.cc b/extern/ceres/internal/ceres/coordinate_descent_minimizer.cc new file mode 100644 index 00000000000..c6b42cf1516 --- /dev/null +++ b/extern/ceres/internal/ceres/coordinate_descent_minimizer.cc @@ -0,0 +1,278 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/coordinate_descent_minimizer.h" + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +#include <iterator> +#include <numeric> +#include <vector> +#include "ceres/evaluator.h" +#include "ceres/linear_solver.h" +#include "ceres/minimizer.h" +#include "ceres/parameter_block.h" +#include "ceres/parameter_block_ordering.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/solver.h" +#include "ceres/trust_region_minimizer.h" +#include "ceres/trust_region_strategy.h" + +namespace ceres { +namespace internal { + +using std::map; +using std::max; +using std::min; +using std::set; +using std::string; +using std::vector; + +CoordinateDescentMinimizer::~CoordinateDescentMinimizer() { +} + +bool CoordinateDescentMinimizer::Init( + const Program& program, + const ProblemImpl::ParameterMap& parameter_map, + const ParameterBlockOrdering& ordering, + string* error) { + parameter_blocks_.clear(); + independent_set_offsets_.clear(); + independent_set_offsets_.push_back(0); + + // Serialize the OrderedGroups into a vector of parameter block + // offsets for parallel access. + map<ParameterBlock*, int> parameter_block_index; + map<int, set<double*> > group_to_elements = ordering.group_to_elements(); + for (map<int, set<double*> >::const_iterator it = group_to_elements.begin(); + it != group_to_elements.end(); + ++it) { + for (set<double*>::const_iterator ptr_it = it->second.begin(); + ptr_it != it->second.end(); + ++ptr_it) { + parameter_blocks_.push_back(parameter_map.find(*ptr_it)->second); + parameter_block_index[parameter_blocks_.back()] = + parameter_blocks_.size() - 1; + } + independent_set_offsets_.push_back( + independent_set_offsets_.back() + it->second.size()); + } + + // The ordering does not have to contain all parameter blocks, so + // assign zero offsets/empty independent sets to these parameter + // blocks. + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + if (!ordering.IsMember(parameter_blocks[i]->mutable_user_state())) { + parameter_blocks_.push_back(parameter_blocks[i]); + independent_set_offsets_.push_back(independent_set_offsets_.back()); + } + } + + // Compute the set of residual blocks that depend on each parameter + // block. + residual_blocks_.resize(parameter_block_index.size()); + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + const map<ParameterBlock*, int>::const_iterator it = + parameter_block_index.find(parameter_block); + if (it != parameter_block_index.end()) { + residual_blocks_[it->second].push_back(residual_block); + } + } + } + + evaluator_options_.linear_solver_type = DENSE_QR; + evaluator_options_.num_eliminate_blocks = 0; + evaluator_options_.num_threads = 1; + + return true; +} + +void CoordinateDescentMinimizer::Minimize( + const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary) { + // Set the state and mark all parameter blocks constant. + for (int i = 0; i < parameter_blocks_.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks_[i]; + parameter_block->SetState(parameters + parameter_block->state_offset()); + parameter_block->SetConstant(); + } + + scoped_array<LinearSolver*> linear_solvers( + new LinearSolver*[options.num_threads]); + + LinearSolver::Options linear_solver_options; + linear_solver_options.type = DENSE_QR; + + for (int i = 0; i < options.num_threads; ++i) { + linear_solvers[i] = LinearSolver::Create(linear_solver_options); + } + + for (int i = 0; i < independent_set_offsets_.size() - 1; ++i) { + const int num_problems = + independent_set_offsets_[i + 1] - independent_set_offsets_[i]; + // No point paying the price for an OpemMP call if the set is of + // size zero. + if (num_problems == 0) { + continue; + } + +#ifdef CERES_USE_OPENMP + const int num_inner_iteration_threads = + min(options.num_threads, num_problems); + evaluator_options_.num_threads = + max(1, options.num_threads / num_inner_iteration_threads); + + // The parameter blocks in each independent set can be optimized + // in parallel, since they do not co-occur in any residual block. +#pragma omp parallel for num_threads(num_inner_iteration_threads) +#endif + for (int j = independent_set_offsets_[i]; + j < independent_set_offsets_[i + 1]; + ++j) { +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + + ParameterBlock* parameter_block = parameter_blocks_[j]; + const int old_index = parameter_block->index(); + const int old_delta_offset = parameter_block->delta_offset(); + parameter_block->SetVarying(); + parameter_block->set_index(0); + parameter_block->set_delta_offset(0); + + Program inner_program; + inner_program.mutable_parameter_blocks()->push_back(parameter_block); + *inner_program.mutable_residual_blocks() = residual_blocks_[j]; + + // TODO(sameeragarwal): Better error handling. Right now we + // assume that this is not going to lead to problems of any + // sort. Basically we should be checking for numerical failure + // of some sort. + // + // On the other hand, if the optimization is a failure, that in + // some ways is fine, since it won't change the parameters and + // we are fine. + Solver::Summary inner_summary; + Solve(&inner_program, + linear_solvers[thread_id], + parameters + parameter_block->state_offset(), + &inner_summary); + + parameter_block->set_index(old_index); + parameter_block->set_delta_offset(old_delta_offset); + parameter_block->SetState(parameters + parameter_block->state_offset()); + parameter_block->SetConstant(); + } + } + + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->SetVarying(); + } + + for (int i = 0; i < options.num_threads; ++i) { + delete linear_solvers[i]; + } +} + +// Solve the optimization problem for one parameter block. +void CoordinateDescentMinimizer::Solve(Program* program, + LinearSolver* linear_solver, + double* parameter, + Solver::Summary* summary) { + *summary = Solver::Summary(); + summary->initial_cost = 0.0; + summary->fixed_cost = 0.0; + summary->final_cost = 0.0; + string error; + + Minimizer::Options minimizer_options; + minimizer_options.evaluator.reset( + CHECK_NOTNULL(Evaluator::Create(evaluator_options_, program, &error))); + minimizer_options.jacobian.reset( + CHECK_NOTNULL(minimizer_options.evaluator->CreateJacobian())); + + TrustRegionStrategy::Options trs_options; + trs_options.linear_solver = linear_solver; + minimizer_options.trust_region_strategy.reset( + CHECK_NOTNULL(TrustRegionStrategy::Create(trs_options))); + minimizer_options.is_silent = true; + + TrustRegionMinimizer minimizer; + minimizer.Minimize(minimizer_options, parameter, summary); +} + +bool CoordinateDescentMinimizer::IsOrderingValid( + const Program& program, + const ParameterBlockOrdering& ordering, + string* message) { + const map<int, set<double*> >& group_to_elements = + ordering.group_to_elements(); + + // Verify that each group is an independent set + map<int, set<double*> >::const_iterator it = group_to_elements.begin(); + for (; it != group_to_elements.end(); ++it) { + if (!program.IsParameterBlockSetIndependent(it->second)) { + *message = + StringPrintf("The user-provided " + "parameter_blocks_for_inner_iterations does not " + "form an independent set. Group Id: %d", it->first); + return false; + } + } + return true; +} + +// Find a recursive decomposition of the Hessian matrix as a set +// of independent sets of decreasing size and invert it. This +// seems to work better in practice, i.e., Cameras before +// points. +ParameterBlockOrdering* CoordinateDescentMinimizer::CreateOrdering( + const Program& program) { + scoped_ptr<ParameterBlockOrdering> ordering(new ParameterBlockOrdering); + ComputeRecursiveIndependentSetOrdering(program, ordering.get()); + ordering->Reverse(); + return ordering.release(); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/coordinate_descent_minimizer.h b/extern/ceres/internal/ceres/coordinate_descent_minimizer.h new file mode 100644 index 00000000000..25ea04ce622 --- /dev/null +++ b/extern/ceres/internal/ceres/coordinate_descent_minimizer.h @@ -0,0 +1,102 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_COORDINATE_DESCENT_MINIMIZER_H_ +#define CERES_INTERNAL_COORDINATE_DESCENT_MINIMIZER_H_ + +#include <string> +#include <vector> + +#include "ceres/evaluator.h" +#include "ceres/minimizer.h" +#include "ceres/problem_impl.h" +#include "ceres/solver.h" + +namespace ceres { +namespace internal { + +class Program; +class LinearSolver; + +// Given a Program, and a ParameterBlockOrdering which partitions +// (non-exhaustively) the Hessian matrix into independent sets, +// perform coordinate descent on the parameter blocks in the +// ordering. The independent set structure allows for all parameter +// blocks in the same independent set to be optimized in parallel, and +// the order of the independent set determines the order in which the +// parameter block groups are optimized. +// +// The minimizer assumes that none of the parameter blocks in the +// program are constant. +class CoordinateDescentMinimizer : public Minimizer { + public: + bool Init(const Program& program, + const ProblemImpl::ParameterMap& parameter_map, + const ParameterBlockOrdering& ordering, + std::string* error); + + // Minimizer interface. + virtual ~CoordinateDescentMinimizer(); + virtual void Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary); + + // Verify that each group in the ordering forms an independent set. + static bool IsOrderingValid(const Program& program, + const ParameterBlockOrdering& ordering, + std::string* message); + + // Find a recursive decomposition of the Hessian matrix as a set + // of independent sets of decreasing size and invert it. This + // seems to work better in practice, i.e., Cameras before + // points. + static ParameterBlockOrdering* CreateOrdering(const Program& program); + + private: + void Solve(Program* program, + LinearSolver* linear_solver, + double* parameters, + Solver::Summary* summary); + + std::vector<ParameterBlock*> parameter_blocks_; + std::vector<std::vector<ResidualBlock*> > residual_blocks_; + // The optimization is performed in rounds. In each round all the + // parameter blocks that form one independent set are optimized in + // parallel. This array, marks the boundaries of the independent + // sets in parameter_blocks_. + std::vector<int> independent_set_offsets_; + + Evaluator::Options evaluator_options_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COORDINATE_DESCENT_MINIMIZER_H_ diff --git a/extern/ceres/internal/ceres/corrector.cc b/extern/ceres/internal/ceres/corrector.cc new file mode 100644 index 00000000000..720182868c1 --- /dev/null +++ b/extern/ceres/internal/ceres/corrector.cc @@ -0,0 +1,158 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/corrector.h" + +#include <cstddef> +#include <cmath> +#include "ceres/internal/eigen.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +Corrector::Corrector(const double sq_norm, const double rho[3]) { + CHECK_GE(sq_norm, 0.0); + sqrt_rho1_ = sqrt(rho[1]); + + // If sq_norm = 0.0, the correction becomes trivial, the residual + // and the jacobian are scaled by the squareroot of the derivative + // of rho. Handling this case explicitly avoids the divide by zero + // error that would occur below. + // + // The case where rho'' < 0 also gets special handling. Technically + // it shouldn't, and the computation of the scaling should proceed + // as below, however we found in experiments that applying the + // curvature correction when rho'' < 0, which is the case when we + // are in the outlier region slows down the convergence of the + // algorithm significantly. + // + // Thus, we have divided the action of the robustifier into two + // parts. In the inliner region, we do the full second order + // correction which re-wights the gradient of the function by the + // square root of the derivative of rho, and the Gauss-Newton + // Hessian gets both the scaling and the rank-1 curvature + // correction. Normaly, alpha is upper bounded by one, but with this + // change, alpha is bounded above by zero. + // + // Empirically we have observed that the full Triggs correction and + // the clamped correction both start out as very good approximations + // to the loss function when we are in the convex part of the + // function, but as the function starts transitioning from convex to + // concave, the Triggs approximation diverges more and more and + // ultimately becomes linear. The clamped Triggs model however + // remains quadratic. + // + // The reason why the Triggs approximation becomes so poor is + // because the curvature correction that it applies to the gauss + // newton hessian goes from being a full rank correction to a rank + // deficient correction making the inversion of the Hessian fraught + // with all sorts of misery and suffering. + // + // The clamped correction retains its quadratic nature and inverting it + // is always well formed. + if ((sq_norm == 0.0) || (rho[2] <= 0.0)) { + residual_scaling_ = sqrt_rho1_; + alpha_sq_norm_ = 0.0; + return; + } + + // We now require that the first derivative of the loss function be + // positive only if the second derivative is positive. This is + // because when the second derivative is non-positive, we do not use + // the second order correction suggested by BANS and instead use a + // simpler first order strategy which does not use a division by the + // gradient of the loss function. + CHECK_GT(rho[1], 0.0); + + // Calculate the smaller of the two solutions to the equation + // + // 0.5 * alpha^2 - alpha - rho'' / rho' * z'z = 0. + // + // Start by calculating the discriminant D. + const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1]; + + // Since both rho[1] and rho[2] are guaranteed to be positive at + // this point, we know that D > 1.0. + + const double alpha = 1.0 - sqrt(D); + + // Calculate the constants needed by the correction routines. + residual_scaling_ = sqrt_rho1_ / (1 - alpha); + alpha_sq_norm_ = alpha / sq_norm; +} + +void Corrector::CorrectResiduals(const int num_rows, double* residuals) { + DCHECK(residuals != NULL); + // Equation 11 in BANS. + VectorRef(residuals, num_rows) *= residual_scaling_; +} + +void Corrector::CorrectJacobian(const int num_rows, + const int num_cols, + double* residuals, + double* jacobian) { + DCHECK(residuals != NULL); + DCHECK(jacobian != NULL); + + // The common case (rho[2] <= 0). + if (alpha_sq_norm_ == 0.0) { + VectorRef(jacobian, num_rows * num_cols) *= sqrt_rho1_; + return; + } + + // 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]; + } + + 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); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/corrector.h b/extern/ceres/internal/ceres/corrector.h new file mode 100644 index 00000000000..315f012ab1d --- /dev/null +++ b/extern/ceres/internal/ceres/corrector.h @@ -0,0 +1,90 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Class definition for the object that is responsible for applying a +// second order correction to the Gauss-Newton based on the ideas in +// BANS by Triggs et al. + +#ifndef CERES_INTERNAL_CORRECTOR_H_ +#define CERES_INTERNAL_CORRECTOR_H_ + +namespace ceres { +namespace internal { + +// Corrector is responsible for applying the second order correction +// to the residual and jacobian of a least squares problem based on a +// radial robust loss. +// +// The key idea here is to look at the expressions for the robustified +// gauss newton approximation and then take its squareroot to get the +// corresponding corrections to the residual and jacobian. For the +// full expressions see Eq. 10 and 11 in BANS by Triggs et al. +class Corrector { + public: + // The constructor takes the squared norm, the value, the first and + // second derivatives of the LossFunction. It precalculates some of + // the constants that are needed to apply the correction. The + // correction constant alpha is constrained to be smaller than 1, if + // it becomes larger than 1, then it will reverse the sign of the + // residual and the correction. If alpha is equal to 1 will result + // in a divide by zero error. Thus we constrain alpha to be upper + // bounded by 1 - epsilon_. + // + // rho[1] needs to be positive. The constructor will crash if this + // condition is not met. + // + // In practical use CorrectJacobian should always be called before + // CorrectResidual, because the jacobian correction depends on the + // value of the uncorrected residual values. + explicit Corrector(double sq_norm, const double rho[3]); + + // residuals *= sqrt(rho[1]) / (1 - alpha) + void CorrectResiduals(int num_rows, double* residuals); + + // jacobian = sqrt(rho[1]) * jacobian - + // sqrt(rho[1]) * alpha / sq_norm * residuals residuals' * jacobian. + // + // The method assumes that the jacobian has row-major storage. It is + // the caller's responsibility to ensure that the pointer to + // jacobian is not null. + void CorrectJacobian(int num_rows, + int num_cols, + double* residuals, + double* jacobian); + + private: + double sqrt_rho1_; + double residual_scaling_; + double alpha_sq_norm_; +}; +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CORRECTOR_H_ diff --git a/extern/ceres/internal/ceres/covariance.cc b/extern/ceres/internal/ceres/covariance.cc new file mode 100644 index 00000000000..690847945a9 --- /dev/null +++ b/extern/ceres/internal/ceres/covariance.cc @@ -0,0 +1,76 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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.h" + +#include <utility> +#include <vector> +#include "ceres/covariance_impl.h" +#include "ceres/problem.h" +#include "ceres/problem_impl.h" + +namespace ceres { + +using std::pair; +using std::vector; + +Covariance::Covariance(const Covariance::Options& options) { + impl_.reset(new internal::CovarianceImpl(options)); +} + +Covariance::~Covariance() { +} + +bool Covariance::Compute( + const vector<pair<const double*, const double*> >& covariance_blocks, + Problem* problem) { + return impl_->Compute(covariance_blocks, problem->problem_impl_.get()); +} + +bool Covariance::GetCovarianceBlock(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const { + return impl_->GetCovarianceBlockInTangentOrAmbientSpace(parameter_block1, + parameter_block2, + true, // ambient + covariance_block); +} + +bool Covariance::GetCovarianceBlockInTangentSpace( + const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const { + return impl_->GetCovarianceBlockInTangentOrAmbientSpace(parameter_block1, + parameter_block2, + false, // tangent + covariance_block); +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/covariance_impl.cc b/extern/ceres/internal/ceres/covariance_impl.cc new file mode 100644 index 00000000000..3e8302bed55 --- /dev/null +++ b/extern/ceres/internal/ceres/covariance_impl.cc @@ -0,0 +1,757 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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 <cstdlib> +#include <utility> +#include <vector> + +#include "Eigen/SparseCore" +#include "Eigen/SparseQR" +#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 { + +using std::make_pair; +using std::map; +using std::pair; +using std::swap; +using std::vector; + +typedef vector<pair<const double*, const double*> > CovarianceBlocks; + +CovarianceImpl::CovarianceImpl(const Covariance::Options& options) + : options_(options), + is_computed_(false), + is_valid_(false) { +#ifndef CERES_USE_OPENMP + if (options_.num_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads = 1 is supported. Switching " + << "to single threaded mode."; + options_.num_threads = 1; + } +#endif + 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::GetCovarianceBlockInTangentOrAmbientSpace( + const double* original_parameter_block1, + const double* original_parameter_block2, + bool lift_covariance_to_ambient_space, + 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) { + 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(ERROR) << "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 or if the + // user does not want it lifted to the ambient space. + if ((local_param1 == NULL && local_param2 == NULL) || + !lift_covariance_to_ambient_space) { + if (transpose) { + MatrixRef(covariance_block, block2_local_size, block1_local_size) = + cov.block(0, offset, block1_local_size, + block2_local_size).transpose(); + } else { + MatrixRef(covariance_block, block1_local_size, block2_local_size) = + cov.block(0, offset, block1_local_size, block2_local_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); + } + } + + std::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 SUITE_SPARSE_QR: + return ComputeCovarianceValuesUsingSuiteSparseQR(); +#else + LOG(ERROR) << "SuiteSparse is required to use the " + << "SUITE_SPARSE_QR algorithm."; + return false; +#endif + case EIGEN_SPARSE_QR: + return ComputeCovarianceValuesUsingEigenSparseQR(); + default: + LOG(ERROR) << "Unsupported covariance estimation algorithm type: " + << CovarianceAlgorithmTypeToString(options_.algorithm_type); + return false; + } + return false; +} + +bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() { + 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(ERROR) << "Jacobian matrix is rank deficient. " + << "Number of columns: " << cholmod_jacobian.ncol + << " rank: " << rank; + free(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]]; + } + } + + free(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 = std::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(ERROR) << "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; +} + +bool CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR() { + EventLogger event_logger( + "CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR"); + 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"); + + typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix; + + // Convert the matrix to column major order as required by SparseQR. + EigenSparseMatrix sparse_jacobian = + Eigen::MappedSparseMatrix<double, Eigen::RowMajor>( + jacobian.num_rows, jacobian.num_cols, + static_cast<int>(jacobian.values.size()), + jacobian.rows.data(), jacobian.cols.data(), jacobian.values.data()); + event_logger.AddEvent("ConvertToSparseMatrix"); + + Eigen::SparseQR<EigenSparseMatrix, Eigen::COLAMDOrdering<int> > + qr_solver(sparse_jacobian); + event_logger.AddEvent("QRDecomposition"); + + if (qr_solver.info() != Eigen::Success) { + LOG(ERROR) << "Eigen::SparseQR decomposition failed."; + return false; + } + + if (qr_solver.rank() < jacobian.num_cols) { + LOG(ERROR) << "Jacobian matrix is rank deficient. " + << "Number of columns: " << jacobian.num_cols + << " rank: " << qr_solver.rank(); + return false; + } + + const int* rows = covariance_matrix_->rows(); + const int* cols = covariance_matrix_->cols(); + double* values = covariance_matrix_->mutable_values(); + + // Compute the inverse column permutation used by QR factorization. + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> inverse_permutation = + qr_solver.colsPermutation().inverse(); + + // 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_cols = jacobian.num_cols; + 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<int>( + num_cols, + qr_solver.matrixR().innerIndexPtr(), + qr_solver.matrixR().outerIndexPtr(), + &qr_solver.matrixR().data().value(0), + inverse_permutation.indices().coeff(r), + solution); + + // Assign the values of the computed covariance using the + // inverse permutation used in the QR factorization. + for (int idx = row_begin; idx < row_end; ++idx) { + const int c = cols[idx]; + values[idx] = solution[inverse_permutation.indices().coeff(c)]; + } + } + + event_logger.AddEvent("Inverse"); + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/covariance_impl.h b/extern/ceres/internal/ceres/covariance_impl.h new file mode 100644 index 00000000000..eb0cd040666 --- /dev/null +++ b/extern/ceres/internal/ceres/covariance_impl.h @@ -0,0 +1,92 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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 std::vector<std::pair<const double*, + const double*> >& covariance_blocks, + ProblemImpl* problem); + + bool GetCovarianceBlockInTangentOrAmbientSpace( + const double* parameter_block1, + const double* parameter_block2, + bool lift_covariance_to_ambient_space, + double* covariance_block) const; + + bool ComputeCovarianceSparsity( + const std::vector<std::pair<const double*, + const double*> >& covariance_blocks, + ProblemImpl* problem); + + bool ComputeCovarianceValues(); + bool ComputeCovarianceValuesUsingDenseSVD(); + bool ComputeCovarianceValuesUsingSuiteSparseQR(); + bool ComputeCovarianceValuesUsingEigenSparseQR(); + + 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_; + std::map<const double*, int> parameter_block_to_row_index_; + std::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/ceres/internal/ceres/cxsparse.h b/extern/ceres/internal/ceres/cxsparse.h new file mode 100644 index 00000000000..26dd1927a78 --- /dev/null +++ b/extern/ceres/internal/ceres/cxsparse.h @@ -0,0 +1,140 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: strandmark@google.com (Petter Strandmark) + +#ifndef CERES_INTERNAL_CXSPARSE_H_ +#define CERES_INTERNAL_CXSPARSE_H_ + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_NO_CXSPARSE + +#include <vector> +#include "cs.h" + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; +class TripletSparseMatrix; + +// This object provides access to solving linear systems using Cholesky +// factorization with a known symbolic factorization. This features does not +// explicity exist in CXSparse. The methods in the class are nonstatic because +// the class manages internal scratch space. +class CXSparse { + public: + CXSparse(); + ~CXSparse(); + + // Solves a symmetric linear system A * x = b using Cholesky factorization. + // A - The system matrix. + // symbolic_factorization - The symbolic factorization of A. This is obtained + // from AnalyzeCholesky. + // b - The right hand size of the linear equation. This + // array will also recieve the solution. + // Returns false if Cholesky factorization of A fails. + bool SolveCholesky(cs_di* A, cs_dis* symbolic_factorization, double* b); + + // Creates a sparse matrix from a compressed-column form. No memory is + // allocated or copied; the structure A is filled out with info from the + // argument. + cs_di CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A); + + // Creates a new matrix from a triplet form. Deallocate the returned matrix + // with Free. May return NULL if the compression or allocation fails. + cs_di* CreateSparseMatrix(TripletSparseMatrix* A); + + // 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 std::vector<int>& row_blocks, + const std::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); + + private: + // Cached scratch space + CS_ENTRY* scratch_; + int scratch_size_; +}; + +} // namespace internal +} // namespace ceres + +#else // CERES_NO_CXSPARSE + +typedef void cs_dis; + +class CXSparse { + public: + void Free(void* arg) {} +}; +#endif // CERES_NO_CXSPARSE + +#endif // CERES_INTERNAL_CXSPARSE_H_ diff --git a/extern/ceres/internal/ceres/dense_jacobian_writer.h b/extern/ceres/internal/ceres/dense_jacobian_writer.h new file mode 100644 index 00000000000..1b04f383f09 --- /dev/null +++ b/extern/ceres/internal/ceres/dense_jacobian_writer.h @@ -0,0 +1,108 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A jacobian writer that writes to dense Eigen matrices. + +#ifndef CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ + +#include "ceres/casts.h" +#include "ceres/dense_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/scratch_evaluate_preparer.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +class DenseJacobianWriter { + public: + DenseJacobianWriter(Evaluator::Options /* ignored */, + Program* program) + : program_(program) { + } + + // JacobianWriter interface. + + // Since the dense matrix has different layout than that assumed by the cost + // functions, use scratch space to store the jacobians temporarily then copy + // them over to the larger jacobian later. + ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads) { + return ScratchEvaluatePreparer::Create(*program_, num_threads); + } + + SparseMatrix* CreateJacobian() const { + return new DenseSparseMatrix(program_->NumResiduals(), + program_->NumEffectiveParameters(), + true); + } + + void Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* jacobian) { + DenseSparseMatrix* dense_jacobian = down_cast<DenseSparseMatrix*>(jacobian); + const ResidualBlock* residual_block = + program_->residual_blocks()[residual_id]; + int num_parameter_blocks = residual_block->NumParameterBlocks(); + int num_residuals = residual_block->NumResiduals(); + + // Now copy the jacobians for each parameter into the dense jacobian matrix. + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + + // If the parameter block is fixed, then there is nothing to do. + if (parameter_block->IsConstant()) { + continue; + } + + const int parameter_block_size = parameter_block->LocalSize(); + ConstMatrixRef parameter_jacobian(jacobians[j], + num_residuals, + parameter_block_size); + + dense_jacobian->mutable_matrix().block( + residual_offset, + parameter_block->delta_offset(), + num_residuals, + parameter_block_size) = parameter_jacobian; + } + } + + private: + Program* program_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ diff --git a/extern/ceres/internal/ceres/dense_normal_cholesky_solver.cc b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.cc new file mode 100644 index 00000000000..b13cf3fc9f6 --- /dev/null +++ b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.cc @@ -0,0 +1,166 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/dense_normal_cholesky_solver.h" + +#include <cstddef> + +#include "Eigen/Dense" +#include "ceres/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" + +namespace ceres { +namespace internal { + +DenseNormalCholeskySolver::DenseNormalCholeskySolver( + const LinearSolver::Options& options) + : options_(options) {} + +LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + 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(); + const int num_cols = A->num_cols(); + + ConstColMajorMatrixRef Aref = A->matrix(); + Matrix lhs(num_cols, num_cols); + lhs.setZero(); + + event_logger.AddEvent("Setup"); + + // lhs += A'A + // + // Using rankUpdate instead of GEMM, exposes the fact that its the + // same matrix being multiplied with itself and that the product is + // symmetric. + lhs.selfadjointView<Eigen::Upper>().rankUpdate(Aref.transpose()); + + // rhs = A'b + Vector rhs = Aref.transpose() * ConstVectorRef(b, num_rows); + + if (per_solve_options.D != NULL) { + ConstVectorRef D(per_solve_options.D, num_cols); + lhs += D.array().square().matrix().asDiagonal(); + } + event_logger.AddEvent("Product"); + + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + Eigen::LLT<Matrix, Eigen::Upper> llt = + lhs.selfadjointView<Eigen::Upper>().llt(); + + if (llt.info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Eigen LLT decomposition failed."; + } else { + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + } + + VectorRef(x, num_cols) = 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"); + + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = + LAPACK::SolveInPlaceUsingCholesky(num_cols, + lhs.data(), + x, + &summary.message); + event_logger.AddEvent("Solve"); + return summary; +} +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/dense_normal_cholesky_solver.h b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.h new file mode 100644 index 00000000000..11287ebf675 --- /dev/null +++ b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.h @@ -0,0 +1,107 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Solve dense rectangular systems Ax = b by forming the normal +// equations and solving them using the Cholesky factorization. + +#ifndef CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_SOLVER_H_ +#define CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class DenseSparseMatrix; + +// This class implements the LinearSolver interface for solving +// rectangular/unsymmetric (well constrained) linear systems of the +// form +// +// Ax = b +// +// Since there does not usually exist a solution that satisfies these +// equations, the solver instead solves the linear least squares +// problem +// +// min_x |Ax - b|^2 +// +// Setting the gradient of the above optimization problem to zero +// gives us the normal equations +// +// A'Ax = A'b +// +// A'A is a positive definite matrix (hopefully), and the resulting +// linear system can be solved using Cholesky factorization. +// +// If the PerSolveOptions struct has a non-null array D, then the +// augmented/regularized linear system +// +// [ A ]x = [b] +// [ diag(D) ] [0] +// +// is solved. +// +// This class uses the LDLT factorization routines from the Eigen +// library. This solver always returns a solution, it is the user's +// responsibility to judge if the solution is good enough for their +// purposes. +class DenseNormalCholeskySolver: public DenseSparseMatrixSolver { + public: + explicit DenseNormalCholeskySolver(const LinearSolver::Options& options); + + private: + virtual LinearSolver::Summary SolveImpl( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + 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); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/dense_qr_solver.cc b/extern/ceres/internal/ceres/dense_qr_solver.cc new file mode 100644 index 00000000000..e85fdfc0c68 --- /dev/null +++ b/extern/ceres/internal/ceres/dense_qr_solver.cc @@ -0,0 +1,170 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/dense_qr_solver.h" + + +#include <cstddef> +#include "Eigen/Dense" +#include "ceres/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" + +namespace ceres { +namespace internal { + +DenseQRSolver::DenseQRSolver(const LinearSolver::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); + } + + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = LAPACK::SolveInPlaceUsingQR(lhs_.rows(), + lhs_.cols(), + lhs_.data(), + work_.rows(), + work_.data(), + rhs_.data(), + &summary.message); + event_logger.AddEvent("Solve"); + if (summary.termination_type == LINEAR_SOLVER_SUCCESS) { + VectorRef(x, num_cols) = rhs_.head(num_cols); + } + + 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(); + 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); + } + + // rhs = [b;0] to account for the additional rows in the lhs. + const int augmented_num_rows = + num_rows + ((per_solve_options.D != NULL) ? num_cols : 0); + if (rhs_.rows() != augmented_num_rows) { + rhs_.resize(augmented_num_rows); + rhs_.setZero(); + } + rhs_.head(num_rows) = ConstVectorRef(b, num_rows); + event_logger.AddEvent("Setup"); + + // Solve the system. + VectorRef(x, num_cols) = A->matrix().householderQr().solve(rhs_); + event_logger.AddEvent("Solve"); + + if (per_solve_options.D != NULL) { + // Undo the modifications to the matrix A. + A->RemoveDiagonal(); + } + + // We always succeed, since the QR solver returns the best solution + // it can. It is the job of the caller to determine if the solution + // is good enough or not. + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + event_logger.AddEvent("TearDown"); + return summary; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/dense_qr_solver.h b/extern/ceres/internal/ceres/dense_qr_solver.h new file mode 100644 index 00000000000..1a6e0898c56 --- /dev/null +++ b/extern/ceres/internal/ceres/dense_qr_solver.h @@ -0,0 +1,115 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Solve dense rectangular systems Ax = b using the QR factorization. +#ifndef CERES_INTERNAL_DENSE_QR_SOLVER_H_ +#define CERES_INTERNAL_DENSE_QR_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class DenseSparseMatrix; + +// This class implements the LinearSolver interface for solving +// rectangular/unsymmetric (well constrained) linear systems of the +// form +// +// Ax = b +// +// Since there does not usually exist a solution that satisfies these +// equations, the solver instead solves the linear least squares +// problem +// +// min_x |Ax - b|^2 +// +// The solution strategy is based on computing the QR decomposition of +// A, i.e. +// +// A = QR +// +// Where Q is an orthonormal matrix and R is an upper triangular +// matrix. Then +// +// Ax = b +// QRx = b +// Q'QRx = Q'b +// Rx = Q'b +// x = R^{-1} Q'b +// +// If the PerSolveOptions struct has a non-null array D, then the +// augmented/regularized linear system +// +// [ A ]x = [b] +// [ diag(D) ] [0] +// +// is solved. +// +// This class uses the dense QR factorization routines from the Eigen +// library. This solver always returns a solution, it is the user's +// responsibility to judge if the solution is good enough for their +// purposes. +class DenseQRSolver: public DenseSparseMatrixSolver { + public: + explicit DenseQRSolver(const LinearSolver::Options& options); + + private: + virtual LinearSolver::Summary SolveImpl( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + 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); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_QR_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/dense_sparse_matrix.cc b/extern/ceres/internal/ceres/dense_sparse_matrix.cc new file mode 100644 index 00000000000..19db867d4aa --- /dev/null +++ b/extern/ceres/internal/ceres/dense_sparse_matrix.cc @@ -0,0 +1,183 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/dense_sparse_matrix.h" + +#include <algorithm> +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +DenseSparseMatrix::DenseSparseMatrix(int num_rows, int num_cols) + : has_diagonal_appended_(false), + has_diagonal_reserved_(false) { + m_.resize(num_rows, num_cols); + m_.setZero(); +} + +DenseSparseMatrix::DenseSparseMatrix(int num_rows, + int num_cols, + bool reserve_diagonal) + : has_diagonal_appended_(false), + has_diagonal_reserved_(reserve_diagonal) { + if (reserve_diagonal) { + // Allocate enough space for the diagonal. + m_.resize(num_rows + num_cols, num_cols); + } else { + m_.resize(num_rows, num_cols); + } + m_.setZero(); +} + +DenseSparseMatrix::DenseSparseMatrix(const TripletSparseMatrix& m) + : m_(Eigen::MatrixXd::Zero(m.num_rows(), m.num_cols())), + has_diagonal_appended_(false), + has_diagonal_reserved_(false) { + const double *values = m.values(); + const int *rows = m.rows(); + const int *cols = m.cols(); + int num_nonzeros = m.num_nonzeros(); + + for (int i = 0; i < num_nonzeros; ++i) { + m_(rows[i], cols[i]) += values[i]; + } +} + +DenseSparseMatrix::DenseSparseMatrix(const ColMajorMatrix& m) + : m_(m), + has_diagonal_appended_(false), + has_diagonal_reserved_(false) { +} + +void DenseSparseMatrix::SetZero() { + m_.setZero(); +} + +void DenseSparseMatrix::RightMultiply(const double* x, double* y) const { + VectorRef(y, num_rows()) += matrix() * ConstVectorRef(x, num_cols()); +} + +void DenseSparseMatrix::LeftMultiply(const double* x, double* y) const { + VectorRef(y, num_cols()) += + matrix().transpose() * ConstVectorRef(x, num_rows()); +} + +void DenseSparseMatrix::SquaredColumnNorm(double* x) const { + VectorRef(x, num_cols()) = m_.colwise().squaredNorm(); +} + +void DenseSparseMatrix::ScaleColumns(const double* scale) { + m_ *= ConstVectorRef(scale, num_cols()).asDiagonal(); +} + +void DenseSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + *dense_matrix = m_.block(0, 0, num_rows(), num_cols()); +} + +void DenseSparseMatrix::AppendDiagonal(double *d) { + CHECK(!has_diagonal_appended_); + if (!has_diagonal_reserved_) { + ColMajorMatrix tmp = m_; + m_.resize(m_.rows() + m_.cols(), m_.cols()); + m_.setZero(); + m_.block(0, 0, tmp.rows(), tmp.cols()) = tmp; + has_diagonal_reserved_ = true; + } + + m_.bottomLeftCorner(m_.cols(), m_.cols()) = + ConstVectorRef(d, m_.cols()).asDiagonal(); + has_diagonal_appended_ = true; +} + +void DenseSparseMatrix::RemoveDiagonal() { + CHECK(has_diagonal_appended_); + has_diagonal_appended_ = false; + // Leave the diagonal reserved. +} + +int DenseSparseMatrix::num_rows() const { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return m_.rows() - m_.cols(); + } + return m_.rows(); +} + +int DenseSparseMatrix::num_cols() const { + return m_.cols(); +} + +int DenseSparseMatrix::num_nonzeros() const { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return (m_.rows() - m_.cols()) * m_.cols(); + } + return m_.rows() * m_.cols(); +} + +ConstColMajorMatrixRef DenseSparseMatrix::matrix() const { + return ConstColMajorMatrixRef( + m_.data(), + ((has_diagonal_reserved_ && !has_diagonal_appended_) + ? m_.rows() - m_.cols() + : m_.rows()), + m_.cols(), + Eigen::Stride<Eigen::Dynamic, 1>(m_.rows(), 1)); +} + +ColMajorMatrixRef DenseSparseMatrix::mutable_matrix() { + return ColMajorMatrixRef( + m_.data(), + ((has_diagonal_reserved_ && !has_diagonal_appended_) + ? m_.rows() - m_.cols() + : m_.rows()), + m_.cols(), + Eigen::Stride<Eigen::Dynamic, 1>(m_.rows(), 1)); +} + + +void DenseSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + const int active_rows = + (has_diagonal_reserved_ && !has_diagonal_appended_) + ? (m_.rows() - m_.cols()) + : m_.rows(); + + for (int r = 0; r < active_rows; ++r) { + for (int c = 0; c < m_.cols(); ++c) { + fprintf(file, "% 10d % 10d %17f\n", r, c, m_(r, c)); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/dense_sparse_matrix.h b/extern/ceres/internal/ceres/dense_sparse_matrix.h new file mode 100644 index 00000000000..b011bfddee7 --- /dev/null +++ b/extern/ceres/internal/ceres/dense_sparse_matrix.h @@ -0,0 +1,109 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A dense matrix implemented under the SparseMatrix interface. + +#ifndef CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ + +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class TripletSparseMatrix; + +class DenseSparseMatrix : public SparseMatrix { + public: + // Build a matrix with the same content as the TripletSparseMatrix + // m. This assumes that m does not have any repeated entries. + explicit DenseSparseMatrix(const TripletSparseMatrix& m); + explicit DenseSparseMatrix(const ColMajorMatrix& m); + + DenseSparseMatrix(int num_rows, int num_cols); + DenseSparseMatrix(int num_rows, int num_cols, bool reserve_diagonal); + + virtual ~DenseSparseMatrix() {} + + // SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; + virtual void ToTextFile(FILE* file) const; + virtual int num_rows() const; + virtual int num_cols() const; + virtual int num_nonzeros() const; + virtual const double* values() const { return m_.data(); } + virtual double* mutable_values() { return m_.data(); } + + ConstColMajorMatrixRef matrix() const; + ColMajorMatrixRef mutable_matrix(); + + // Only one diagonal can be appended at a time. The diagonal is appended to + // as a new set of rows, e.g. + // + // Original matrix: + // + // x x x + // x x x + // x x x + // + // After append diagonal (1, 2, 3): + // + // x x x + // x x x + // x x x + // 1 0 0 + // 0 2 0 + // 0 0 3 + // + // Calling RemoveDiagonal removes the block. It is a fatal error to append a + // diagonal to a matrix that already has an appended diagonal, and it is also + // a fatal error to remove a diagonal from a matrix that has none. + void AppendDiagonal(double *d); + void RemoveDiagonal(); + + private: + ColMajorMatrix m_; + bool has_diagonal_appended_; + bool has_diagonal_reserved_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/detect_structure.cc b/extern/ceres/internal/ceres/detect_structure.cc new file mode 100644 index 00000000000..959a0ee3c84 --- /dev/null +++ b/extern/ceres/internal/ceres/detect_structure.cc @@ -0,0 +1,120 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/detect_structure.h" +#include "ceres/internal/eigen.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +void DetectStructure(const CompressedRowBlockStructure& bs, + const int num_eliminate_blocks, + int* row_block_size, + int* e_block_size, + int* f_block_size) { + const int num_row_blocks = bs.rows.size(); + *row_block_size = 0; + *e_block_size = 0; + *f_block_size = 0; + + // Iterate over row blocks of the matrix, checking if row_block, + // e_block or f_block sizes remain constant. + for (int r = 0; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + // We do not care about the sizes of the blocks in rows which do + // not contain e_blocks. + if (row.cells.front().block_id >= num_eliminate_blocks) { + break; + } + + // Detect fixed or dynamic row block size. + if (*row_block_size == 0) { + *row_block_size = row.block.size; + } else if (*row_block_size != Eigen::Dynamic && + *row_block_size != row.block.size) { + VLOG(2) << "Dynamic row block size because the block size changed from " + << *row_block_size << " to " + << row.block.size; + *row_block_size = Eigen::Dynamic; + } + + // Detect fixed or dynamic e-block size. + const int e_block_id = row.cells.front().block_id; + if (*e_block_size == 0) { + *e_block_size = bs.cols[e_block_id].size; + } else if (*e_block_size != Eigen::Dynamic && + *e_block_size != bs.cols[e_block_id].size) { + VLOG(2) << "Dynamic e block size because the block size changed from " + << *e_block_size << " to " + << bs.cols[e_block_id].size; + *e_block_size = Eigen::Dynamic; + } + + // Detect fixed or dynamic f-block size. We are only interested in + // rows with e-blocks, and the e-block is always the first block, + // so only rows of size greater than 1 are of interest. + if (row.cells.size() > 1) { + if (*f_block_size == 0) { + const int f_block_id = row.cells[1].block_id; + *f_block_size = bs.cols[f_block_id].size; + } + + for (int c = 1; + (c < row.cells.size()) && (*f_block_size != Eigen::Dynamic); + ++c) { + const int f_block_id = row.cells[c].block_id; + if (*f_block_size != bs.cols[f_block_id].size) { + VLOG(2) << "Dynamic f block size because the block size " + << "changed from " << *f_block_size << " to " + << bs.cols[f_block_id].size; + *f_block_size = Eigen::Dynamic; + } + } + } + + const bool is_everything_dynamic = (*row_block_size == Eigen::Dynamic && + *e_block_size == Eigen::Dynamic && + *f_block_size == Eigen::Dynamic); + if (is_everything_dynamic) { + break; + } + } + + CHECK_NE(*row_block_size, 0) << "No rows found"; + CHECK_NE(*e_block_size, 0) << "No e type blocks found"; + VLOG(1) << "Schur complement static structure <" + << *row_block_size << "," + << *e_block_size << "," + << *f_block_size << ">."; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/detect_structure.h b/extern/ceres/internal/ceres/detect_structure.h new file mode 100644 index 00000000000..602581c846e --- /dev/null +++ b/extern/ceres/internal/ceres/detect_structure.h @@ -0,0 +1,67 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_DETECT_STRUCTURE_H_ +#define CERES_INTERNAL_DETECT_STRUCTURE_H_ + +#include "ceres/block_structure.h" + +namespace ceres { +namespace internal { + +// Detect static blocks in the problem sparsity. For rows containing +// e_blocks, we are interested in detecting if the size of the row +// blocks, e_blocks and the f_blocks remain constant. If they do, then +// we can use template specialization to improve the performance of +// the block level linear algebra operations used by the +// SchurEliminator. +// +// If a block size is not constant, we return Eigen::Dynamic as the +// value. This just means that the eliminator uses dynamically sized +// linear algebra operations rather than static operations whose size +// is known as compile time. +// +// For more details about e_blocks and f_blocks, see +// schur_eliminator.h. This information is used to initialized an +// appropriate template specialization of SchurEliminator. +// +// Note: The structure of rows without any e-blocks has no effect on +// the values returned by this function. It is entirely possible that +// the f_block_size and row_blocks_size is not constant in such rows. +void DetectStructure(const CompressedRowBlockStructure& bs, + const int num_eliminate_blocks, + int* row_block_size, + int* e_block_size, + int* f_block_size); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DETECT_STRUCTURE_H_ diff --git a/extern/ceres/internal/ceres/dogleg_strategy.cc b/extern/ceres/internal/ceres/dogleg_strategy.cc new file mode 100644 index 00000000000..839e1816338 --- /dev/null +++ b/extern/ceres/internal/ceres/dogleg_strategy.cc @@ -0,0 +1,718 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/dogleg_strategy.h" + +#include <cmath> +#include "Eigen/Dense" +#include "ceres/array_utils.h" +#include "ceres/internal/eigen.h" +#include "ceres/linear_least_squares_problems.h" +#include "ceres/linear_solver.h" +#include "ceres/polynomial.h" +#include "ceres/sparse_matrix.h" +#include "ceres/trust_region_strategy.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { +const double kMaxMu = 1.0; +const double kMinMu = 1e-8; +} + +DoglegStrategy::DoglegStrategy(const TrustRegionStrategy::Options& options) + : linear_solver_(options.linear_solver), + radius_(options.initial_radius), + max_radius_(options.max_radius), + min_diagonal_(options.min_lm_diagonal), + max_diagonal_(options.max_lm_diagonal), + mu_(kMinMu), + min_mu_(kMinMu), + max_mu_(kMaxMu), + mu_increase_factor_(10.0), + increase_threshold_(0.75), + decrease_threshold_(0.25), + dogleg_step_norm_(0.0), + reuse_(false), + dogleg_type_(options.dogleg_type) { + CHECK_NOTNULL(linear_solver_); + CHECK_GT(min_diagonal_, 0.0); + CHECK_LE(min_diagonal_, max_diagonal_); + CHECK_GT(max_radius_, 0.0); +} + +// If the reuse_ flag is not set, then the Cauchy point (scaled +// gradient) and the new Gauss-Newton step are computed from +// scratch. The Dogleg step is then computed as interpolation of these +// two vectors. +TrustRegionStrategy::Summary DoglegStrategy::ComputeStep( + const TrustRegionStrategy::PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals, + double* step) { + CHECK_NOTNULL(jacobian); + CHECK_NOTNULL(residuals); + CHECK_NOTNULL(step); + + const int n = jacobian->num_cols(); + if (reuse_) { + // Gauss-Newton and gradient vectors are always available, only a + // new interpolant need to be computed. For the subspace case, + // the subspace and the two-dimensional model are also still valid. + switch (dogleg_type_) { + case TRADITIONAL_DOGLEG: + ComputeTraditionalDoglegStep(step); + break; + + case SUBSPACE_DOGLEG: + ComputeSubspaceDoglegStep(step); + break; + } + TrustRegionStrategy::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + return summary; + } + + reuse_ = true; + // Check that we have the storage needed to hold the various + // temporary vectors. + if (diagonal_.rows() != n) { + diagonal_.resize(n, 1); + gradient_.resize(n, 1); + gauss_newton_step_.resize(n, 1); + } + + // Vector used to form the diagonal matrix that is used to + // regularize the Gauss-Newton solve and that defines the + // elliptical trust region + // + // || D * step || <= radius_ . + // + jacobian->SquaredColumnNorm(diagonal_.data()); + for (int i = 0; i < n; ++i) { + diagonal_[i] = std::min(std::max(diagonal_[i], min_diagonal_), + max_diagonal_); + } + diagonal_ = diagonal_.array().sqrt(); + + ComputeGradient(jacobian, residuals); + ComputeCauchyPoint(jacobian); + + LinearSolver::Summary linear_solver_summary = + ComputeGaussNewtonStep(per_solve_options, jacobian, residuals); + + TrustRegionStrategy::Summary summary; + summary.residual_norm = linear_solver_summary.residual_norm; + summary.num_iterations = linear_solver_summary.num_iterations; + summary.termination_type = linear_solver_summary.termination_type; + + if (linear_solver_summary.termination_type == LINEAR_SOLVER_FATAL_ERROR) { + return summary; + } + + if (linear_solver_summary.termination_type != LINEAR_SOLVER_FAILURE) { + switch (dogleg_type_) { + // Interpolate the Cauchy point and the Gauss-Newton step. + case TRADITIONAL_DOGLEG: + ComputeTraditionalDoglegStep(step); + break; + + // Find the minimum in the subspace defined by the + // Cauchy point and the (Gauss-)Newton step. + case SUBSPACE_DOGLEG: + if (!ComputeSubspaceModel(jacobian)) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + break; + } + ComputeSubspaceDoglegStep(step); + break; + } + } + + return summary; +} + +// The trust region is assumed to be elliptical with the +// diagonal scaling matrix D defined by sqrt(diagonal_). +// It is implemented by substituting step' = D * step. +// The trust region for step' is spherical. +// The gradient, the Gauss-Newton step, the Cauchy point, +// and all calculations involving the Jacobian have to +// be adjusted accordingly. +void DoglegStrategy::ComputeGradient( + SparseMatrix* jacobian, + const double* residuals) { + gradient_.setZero(); + jacobian->LeftMultiply(residuals, gradient_.data()); + gradient_.array() /= diagonal_.array(); +} + +// The Cauchy point is the global minimizer of the quadratic model +// along the one-dimensional subspace spanned by the gradient. +void DoglegStrategy::ComputeCauchyPoint(SparseMatrix* jacobian) { + // alpha * -gradient is the Cauchy point. + Vector Jg(jacobian->num_rows()); + Jg.setZero(); + // The Jacobian is scaled implicitly by computing J * (D^-1 * (D^-1 * g)) + // instead of (J * D^-1) * (D^-1 * g). + Vector scaled_gradient = + (gradient_.array() / diagonal_.array()).matrix(); + jacobian->RightMultiply(scaled_gradient.data(), Jg.data()); + alpha_ = gradient_.squaredNorm() / Jg.squaredNorm(); +} + +// The dogleg step is defined as the intersection of the trust region +// boundary with the piecewise linear path from the origin to the Cauchy +// point and then from there to the Gauss-Newton point (global minimizer +// of the model function). The Gauss-Newton point is taken if it lies +// within the trust region. +void DoglegStrategy::ComputeTraditionalDoglegStep(double* dogleg) { + VectorRef dogleg_step(dogleg, gradient_.rows()); + + // Case 1. The Gauss-Newton step lies inside the trust region, and + // is therefore the optimal solution to the trust-region problem. + const double gradient_norm = gradient_.norm(); + const double gauss_newton_norm = gauss_newton_step_.norm(); + if (gauss_newton_norm <= radius_) { + dogleg_step = gauss_newton_step_; + dogleg_step_norm_ = gauss_newton_norm; + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "GaussNewton step size: " << dogleg_step_norm_ + << " radius: " << radius_; + return; + } + + // Case 2. The Cauchy point and the Gauss-Newton steps lie outside + // the trust region. Rescale the Cauchy point to the trust region + // and return. + if (gradient_norm * alpha_ >= radius_) { + dogleg_step = -(radius_ / gradient_norm) * gradient_; + dogleg_step_norm_ = radius_; + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "Cauchy step size: " << dogleg_step_norm_ + << " radius: " << radius_; + return; + } + + // Case 3. The Cauchy point is inside the trust region and the + // Gauss-Newton step is outside. Compute the line joining the two + // points and the point on it which intersects the trust region + // boundary. + + // a = alpha * -gradient + // b = gauss_newton_step + const double b_dot_a = -alpha_ * gradient_.dot(gauss_newton_step_); + const double a_squared_norm = pow(alpha_ * gradient_norm, 2.0); + const double b_minus_a_squared_norm = + a_squared_norm - 2 * b_dot_a + pow(gauss_newton_norm, 2); + + // c = a' (b - a) + // = alpha * -gradient' gauss_newton_step - alpha^2 |gradient|^2 + const double c = b_dot_a - a_squared_norm; + const double d = sqrt(c * c + b_minus_a_squared_norm * + (pow(radius_, 2.0) - a_squared_norm)); + + double beta = + (c <= 0) + ? (d - c) / b_minus_a_squared_norm + : (radius_ * radius_ - a_squared_norm) / (d + c); + dogleg_step = (-alpha_ * (1.0 - beta)) * gradient_ + + beta * gauss_newton_step_; + dogleg_step_norm_ = dogleg_step.norm(); + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "Dogleg step size: " << dogleg_step_norm_ + << " radius: " << radius_; +} + +// The subspace method finds the minimum of the two-dimensional problem +// +// min. 1/2 x' B' H B x + g' B x +// s.t. || B x ||^2 <= r^2 +// +// where r is the trust region radius and B is the matrix with unit columns +// spanning the subspace defined by the steepest descent and Newton direction. +// This subspace by definition includes the Gauss-Newton point, which is +// therefore taken if it lies within the trust region. +void DoglegStrategy::ComputeSubspaceDoglegStep(double* dogleg) { + VectorRef dogleg_step(dogleg, gradient_.rows()); + + // The Gauss-Newton point is inside the trust region if |GN| <= radius_. + // This test is valid even though radius_ is a length in the two-dimensional + // subspace while gauss_newton_step_ is expressed in the (scaled) + // higher dimensional original space. This is because + // + // 1. gauss_newton_step_ by definition lies in the subspace, and + // 2. the subspace basis is orthonormal. + // + // As a consequence, the norm of the gauss_newton_step_ in the subspace is + // the same as its norm in the original space. + const double gauss_newton_norm = gauss_newton_step_.norm(); + if (gauss_newton_norm <= radius_) { + dogleg_step = gauss_newton_step_; + dogleg_step_norm_ = gauss_newton_norm; + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "GaussNewton step size: " << dogleg_step_norm_ + << " radius: " << radius_; + return; + } + + // The optimum lies on the boundary of the trust region. The above problem + // therefore becomes + // + // min. 1/2 x^T B^T H B x + g^T B x + // s.t. || B x ||^2 = r^2 + // + // Notice the equality in the constraint. + // + // This can be solved by forming the Lagrangian, solving for x(y), where + // y is the Lagrange multiplier, using the gradient of the objective, and + // putting x(y) back into the constraint. This results in a fourth order + // polynomial in y, which can be solved using e.g. the companion matrix. + // See the description of MakePolynomialForBoundaryConstrainedProblem for + // details. The result is up to four real roots y*, not all of which + // correspond to feasible points. The feasible points x(y*) have to be + // tested for optimality. + + if (subspace_is_one_dimensional_) { + // The subspace is one-dimensional, so both the gradient and + // the Gauss-Newton step point towards the same direction. + // In this case, we move along the gradient until we reach the trust + // region boundary. + dogleg_step = -(radius_ / gradient_.norm()) * gradient_; + dogleg_step_norm_ = radius_; + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "Dogleg subspace step size (1D): " << dogleg_step_norm_ + << " radius: " << radius_; + return; + } + + Vector2d minimum(0.0, 0.0); + if (!FindMinimumOnTrustRegionBoundary(&minimum)) { + // For the positive semi-definite case, a traditional dogleg step + // is taken in this case. + LOG(WARNING) << "Failed to compute polynomial roots. " + << "Taking traditional dogleg step instead."; + ComputeTraditionalDoglegStep(dogleg); + return; + } + + // Test first order optimality at the minimum. + // The first order KKT conditions state that the minimum x* + // has to satisfy either || x* ||^2 < r^2 (i.e. has to lie within + // the trust region), or + // + // (B x* + g) + y x* = 0 + // + // for some positive scalar y. + // Here, as it is already known that the minimum lies on the boundary, the + // latter condition is tested. To allow for small imprecisions, we test if + // the angle between (B x* + g) and -x* is smaller than acos(0.99). + // The exact value of the cosine is arbitrary but should be close to 1. + // + // This condition should not be violated. If it is, the minimum was not + // correctly determined. + const double kCosineThreshold = 0.99; + const Vector2d grad_minimum = subspace_B_ * minimum + subspace_g_; + const double cosine_angle = -minimum.dot(grad_minimum) / + (minimum.norm() * grad_minimum.norm()); + if (cosine_angle < kCosineThreshold) { + LOG(WARNING) << "First order optimality seems to be violated " + << "in the subspace method!\n" + << "Cosine of angle between x and B x + g is " + << cosine_angle << ".\n" + << "Taking a regular dogleg step instead.\n" + << "Please consider filing a bug report if this " + << "happens frequently or consistently.\n"; + ComputeTraditionalDoglegStep(dogleg); + return; + } + + // Create the full step from the optimal 2d solution. + dogleg_step = subspace_basis_ * minimum; + dogleg_step_norm_ = radius_; + dogleg_step.array() /= diagonal_.array(); + VLOG(3) << "Dogleg subspace step size: " << dogleg_step_norm_ + << " radius: " << radius_; +} + +// Build the polynomial that defines the optimal Lagrange multipliers. +// Let the Lagrangian be +// +// L(x, y) = 0.5 x^T B x + x^T g + y (0.5 x^T x - 0.5 r^2). (1) +// +// Stationary points of the Lagrangian are given by +// +// 0 = d L(x, y) / dx = Bx + g + y x (2) +// 0 = d L(x, y) / dy = 0.5 x^T x - 0.5 r^2 (3) +// +// For any given y, we can solve (2) for x as +// +// x(y) = -(B + y I)^-1 g . (4) +// +// As B + y I is 2x2, we form the inverse explicitly: +// +// (B + y I)^-1 = (1 / det(B + y I)) adj(B + y I) (5) +// +// where adj() denotes adjugation. This should be safe, as B is positive +// semi-definite and y is necessarily positive, so (B + y I) is indeed +// invertible. +// Plugging (5) into (4) and the result into (3), then dividing by 0.5 we +// obtain +// +// 0 = (1 / det(B + y I))^2 g^T adj(B + y I)^T adj(B + y I) g - r^2 +// (6) +// +// or +// +// det(B + y I)^2 r^2 = g^T adj(B + y I)^T adj(B + y I) g (7a) +// = g^T adj(B)^T adj(B) g +// + 2 y g^T adj(B)^T g + y^2 g^T g (7b) +// +// as +// +// adj(B + y I) = adj(B) + y I = adj(B)^T + y I . (8) +// +// The left hand side can be expressed explicitly using +// +// det(B + y I) = det(B) + y tr(B) + y^2 . (9) +// +// So (7) is a polynomial in y of degree four. +// Bringing everything back to the left hand side, the coefficients can +// be read off as +// +// y^4 r^2 +// + y^3 2 r^2 tr(B) +// + y^2 (r^2 tr(B)^2 + 2 r^2 det(B) - g^T g) +// + y^1 (2 r^2 det(B) tr(B) - 2 g^T adj(B)^T g) +// + y^0 (r^2 det(B)^2 - g^T adj(B)^T adj(B) g) +// +Vector DoglegStrategy::MakePolynomialForBoundaryConstrainedProblem() const { + const double detB = subspace_B_.determinant(); + const double trB = subspace_B_.trace(); + const double r2 = radius_ * radius_; + Matrix2d B_adj; + B_adj << subspace_B_(1, 1) , -subspace_B_(0, 1), + -subspace_B_(1, 0) , subspace_B_(0, 0); + + Vector polynomial(5); + polynomial(0) = r2; + polynomial(1) = 2.0 * r2 * trB; + polynomial(2) = r2 * (trB * trB + 2.0 * detB) - subspace_g_.squaredNorm(); + polynomial(3) = -2.0 * (subspace_g_.transpose() * B_adj * subspace_g_ + - r2 * detB * trB); + polynomial(4) = r2 * detB * detB - (B_adj * subspace_g_).squaredNorm(); + + return polynomial; +} + +// Given a Lagrange multiplier y that corresponds to a stationary point +// of the Lagrangian L(x, y), compute the corresponding x from the +// equation +// +// 0 = d L(x, y) / dx +// = B * x + g + y * x +// = (B + y * I) * x + g +// +DoglegStrategy::Vector2d DoglegStrategy::ComputeSubspaceStepFromRoot( + double y) const { + const Matrix2d B_i = subspace_B_ + y * Matrix2d::Identity(); + return -B_i.partialPivLu().solve(subspace_g_); +} + +// This function evaluates the quadratic model at a point x in the +// subspace spanned by subspace_basis_. +double DoglegStrategy::EvaluateSubspaceModel(const Vector2d& x) const { + return 0.5 * x.dot(subspace_B_ * x) + subspace_g_.dot(x); +} + +// This function attempts to solve the boundary-constrained subspace problem +// +// min. 1/2 x^T B^T H B x + g^T B x +// s.t. || B x ||^2 = r^2 +// +// where B is an orthonormal subspace basis and r is the trust-region radius. +// +// This is done by finding the roots of a fourth degree polynomial. If the +// root finding fails, the function returns false and minimum will be set +// to (0, 0). If it succeeds, true is returned. +// +// In the failure case, another step should be taken, such as the traditional +// dogleg step. +bool DoglegStrategy::FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const { + CHECK_NOTNULL(minimum); + + // Return (0, 0) in all error cases. + minimum->setZero(); + + // Create the fourth-degree polynomial that is a necessary condition for + // optimality. + const Vector polynomial = MakePolynomialForBoundaryConstrainedProblem(); + + // Find the real parts y_i of its roots (not only the real roots). + Vector roots_real; + if (!FindPolynomialRoots(polynomial, &roots_real, NULL)) { + // Failed to find the roots of the polynomial, i.e. the candidate + // solutions of the constrained problem. Report this back to the caller. + return false; + } + + // For each root y, compute B x(y) and check for feasibility. + // Notice that there should always be four roots, as the leading term of + // the polynomial is r^2 and therefore non-zero. However, as some roots + // may be complex, the real parts are not necessarily unique. + double minimum_value = std::numeric_limits<double>::max(); + bool valid_root_found = false; + for (int i = 0; i < roots_real.size(); ++i) { + const Vector2d x_i = ComputeSubspaceStepFromRoot(roots_real(i)); + + // Not all roots correspond to points on the trust region boundary. + // There are at most four candidate solutions. As we are interested + // in the minimum, it is safe to consider all of them after projecting + // them onto the trust region boundary. + if (x_i.norm() > 0) { + const double f_i = EvaluateSubspaceModel((radius_ / x_i.norm()) * x_i); + valid_root_found = true; + if (f_i < minimum_value) { + minimum_value = f_i; + *minimum = x_i; + } + } + } + + return valid_root_found; +} + +LinearSolver::Summary DoglegStrategy::ComputeGaussNewtonStep( + const PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals) { + const int n = jacobian->num_cols(); + LinearSolver::Summary linear_solver_summary; + linear_solver_summary.termination_type = LINEAR_SOLVER_FAILURE; + + // The Jacobian matrix is often quite poorly conditioned. Thus it is + // necessary to add a diagonal matrix at the bottom to prevent the + // linear solver from failing. + // + // We do this by computing the same diagonal matrix as the one used + // by Levenberg-Marquardt (other choices are possible), and scaling + // it by a small constant (independent of the trust region radius). + // + // If the solve fails, the multiplier to the diagonal is increased + // up to max_mu_ by a factor of mu_increase_factor_ every time. If + // the linear solver is still not successful, the strategy returns + // with LINEAR_SOLVER_FAILURE. + // + // Next time when a new Gauss-Newton step is requested, the + // multiplier starts out from the last successful solve. + // + // When a step is declared successful, the multiplier is decreased + // by half of mu_increase_factor_. + + while (mu_ < max_mu_) { + // Dogleg, as far as I (sameeragarwal) understand it, requires a + // reasonably good estimate of the Gauss-Newton step. This means + // that we need to solve the normal equations more or less + // exactly. This is reflected in the values of the tolerances set + // below. + // + // For now, this strategy should only be used with exact + // factorization based solvers, for which these tolerances are + // automatically satisfied. + // + // The right way to combine inexact solves with trust region + // methods is to use Stiehaug's method. + LinearSolver::PerSolveOptions solve_options; + solve_options.q_tolerance = 0.0; + solve_options.r_tolerance = 0.0; + + lm_diagonal_ = diagonal_ * std::sqrt(mu_); + solve_options.D = lm_diagonal_.data(); + + // As in the LevenbergMarquardtStrategy, solve Jy = r instead + // of Jx = -r and later set x = -y to avoid having to modify + // either jacobian or residuals. + InvalidateArray(n, gauss_newton_step_.data()); + linear_solver_summary = linear_solver_->Solve(jacobian, + residuals, + solve_options, + gauss_newton_step_.data()); + + if (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 == LINEAR_SOLVER_FATAL_ERROR) { + return linear_solver_summary; + } + + if (linear_solver_summary.termination_type == LINEAR_SOLVER_FAILURE || + !IsArrayValid(n, gauss_newton_step_.data())) { + mu_ *= mu_increase_factor_; + VLOG(2) << "Increasing mu " << mu_; + linear_solver_summary.termination_type = LINEAR_SOLVER_FAILURE; + continue; + } + break; + } + + if (linear_solver_summary.termination_type != LINEAR_SOLVER_FAILURE) { + // The scaled Gauss-Newton step is D * GN: + // + // - (D^-1 J^T J D^-1)^-1 (D^-1 g) + // = - D (J^T J)^-1 D D^-1 g + // = D -(J^T J)^-1 g + // + gauss_newton_step_.array() *= -diagonal_.array(); + } + + return linear_solver_summary; +} + +void DoglegStrategy::StepAccepted(double step_quality) { + CHECK_GT(step_quality, 0.0); + + if (step_quality < decrease_threshold_) { + radius_ *= 0.5; + } + + if (step_quality > increase_threshold_) { + radius_ = std::max(radius_, 3.0 * dogleg_step_norm_); + } + + // Reduce the regularization multiplier, in the hope that whatever + // was causing the rank deficiency has gone away and we can return + // to doing a pure Gauss-Newton solve. + mu_ = std::max(min_mu_, 2.0 * mu_ / mu_increase_factor_); + reuse_ = false; +} + +void DoglegStrategy::StepRejected(double step_quality) { + radius_ *= 0.5; + reuse_ = true; +} + +void DoglegStrategy::StepIsInvalid() { + mu_ *= mu_increase_factor_; + reuse_ = false; +} + +double DoglegStrategy::Radius() const { + return radius_; +} + +bool DoglegStrategy::ComputeSubspaceModel(SparseMatrix* jacobian) { + // Compute an orthogonal basis for the subspace using QR decomposition. + Matrix basis_vectors(jacobian->num_cols(), 2); + basis_vectors.col(0) = gradient_; + basis_vectors.col(1) = gauss_newton_step_; + Eigen::ColPivHouseholderQR<Matrix> basis_qr(basis_vectors); + + switch (basis_qr.rank()) { + case 0: + // This should never happen, as it implies that both the gradient + // and the Gauss-Newton step are zero. In this case, the minimizer should + // have stopped due to the gradient being too small. + LOG(ERROR) << "Rank of subspace basis is 0. " + << "This means that the gradient at the current iterate is " + << "zero but the optimization has not been terminated. " + << "You may have found a bug in Ceres."; + return false; + + case 1: + // Gradient and Gauss-Newton step coincide, so we lie on one of the + // major axes of the quadratic problem. In this case, we simply move + // along the gradient until we reach the trust region boundary. + subspace_is_one_dimensional_ = true; + return true; + + case 2: + subspace_is_one_dimensional_ = false; + break; + + default: + LOG(ERROR) << "Rank of the subspace basis matrix is reported to be " + << "greater than 2. As the matrix contains only two " + << "columns this cannot be true and is indicative of " + << "a bug."; + return false; + } + + // The subspace is two-dimensional, so compute the subspace model. + // Given the basis U, this is + // + // subspace_g_ = g_scaled^T U + // + // and + // + // subspace_B_ = U^T (J_scaled^T J_scaled) U + // + // As J_scaled = J * D^-1, the latter becomes + // + // subspace_B_ = ((U^T D^-1) J^T) (J (D^-1 U)) + // = (J (D^-1 U))^T (J (D^-1 U)) + + subspace_basis_ = + basis_qr.householderQ() * Matrix::Identity(jacobian->num_cols(), 2); + + subspace_g_ = subspace_basis_.transpose() * gradient_; + + Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::RowMajor> + Jb(2, jacobian->num_rows()); + Jb.setZero(); + + Vector tmp; + tmp = (subspace_basis_.col(0).array() / diagonal_.array()).matrix(); + jacobian->RightMultiply(tmp.data(), Jb.row(0).data()); + tmp = (subspace_basis_.col(1).array() / diagonal_.array()).matrix(); + jacobian->RightMultiply(tmp.data(), Jb.row(1).data()); + + subspace_B_ = Jb * Jb.transpose(); + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/dogleg_strategy.h b/extern/ceres/internal/ceres/dogleg_strategy.h new file mode 100644 index 00000000000..046b9d824c9 --- /dev/null +++ b/extern/ceres/internal/ceres/dogleg_strategy.h @@ -0,0 +1,165 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_DOGLEG_STRATEGY_H_ +#define CERES_INTERNAL_DOGLEG_STRATEGY_H_ + +#include "ceres/linear_solver.h" +#include "ceres/trust_region_strategy.h" + +namespace ceres { +namespace internal { + +// Dogleg step computation and trust region sizing strategy based on +// on "Methods for Nonlinear Least Squares" by K. Madsen, H.B. Nielsen +// and O. Tingleff. Available to download from +// +// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf +// +// One minor modification is that instead of computing the pure +// Gauss-Newton step, we compute a regularized version of it. This is +// because the Jacobian is often rank-deficient and in such cases +// using a direct solver leads to numerical failure. +// +// If SUBSPACE is passed as the type argument to the constructor, the +// DoglegStrategy follows the approach by Shultz, Schnabel, Byrd. +// This finds the exact optimum over the two-dimensional subspace +// spanned by the two Dogleg vectors. +class DoglegStrategy : public TrustRegionStrategy { + public: + explicit DoglegStrategy(const TrustRegionStrategy::Options& options); + virtual ~DoglegStrategy() {} + + // TrustRegionStrategy interface + virtual Summary ComputeStep(const PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals, + double* step); + virtual void StepAccepted(double step_quality); + virtual void StepRejected(double step_quality); + virtual void StepIsInvalid(); + + virtual double Radius() const; + + // These functions are predominantly for testing. + Vector gradient() const { return gradient_; } + Vector gauss_newton_step() const { return gauss_newton_step_; } + Matrix subspace_basis() const { return subspace_basis_; } + Vector subspace_g() const { return subspace_g_; } + Matrix subspace_B() const { return subspace_B_; } + + private: + typedef Eigen::Matrix<double, 2, 1, Eigen::DontAlign> Vector2d; + typedef Eigen::Matrix<double, 2, 2, Eigen::DontAlign> Matrix2d; + + LinearSolver::Summary ComputeGaussNewtonStep( + 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); + bool ComputeSubspaceModel(SparseMatrix* jacobian); + void ComputeSubspaceDoglegStep(double* step); + + bool FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const; + Vector MakePolynomialForBoundaryConstrainedProblem() const; + Vector2d ComputeSubspaceStepFromRoot(double lambda) const; + double EvaluateSubspaceModel(const Vector2d& x) const; + + LinearSolver* linear_solver_; + double radius_; + const double max_radius_; + + const double min_diagonal_; + const double max_diagonal_; + + // mu is used to scale the diagonal matrix used to make the + // Gauss-Newton solve full rank. In each solve, the strategy starts + // out with mu = min_mu, and tries values upto max_mu. If the user + // reports an invalid step, the value of mu_ is increased so that + // the next solve starts with a stronger regularization. + // + // If a successful step is reported, then the value of mu_ is + // decreased with a lower bound of min_mu_. + double mu_; + const double min_mu_; + const double max_mu_; + const double mu_increase_factor_; + const double increase_threshold_; + const double decrease_threshold_; + + Vector diagonal_; // sqrt(diag(J^T J)) + Vector lm_diagonal_; + + Vector gradient_; + Vector gauss_newton_step_; + + // cauchy_step = alpha * gradient + double alpha_; + double dogleg_step_norm_; + + // When, ComputeStep is called, reuse_ indicates whether the + // Gauss-Newton and Cauchy steps from the last call to ComputeStep + // can be reused or not. + // + // If the user called StepAccepted, then it is expected that the + // user has recomputed the Jacobian matrix and new Gauss-Newton + // solve is needed and reuse is set to false. + // + // If the user called StepRejected, then it is expected that the + // user wants to solve the trust region problem with the same matrix + // but a different trust region radius and the Gauss-Newton and + // Cauchy steps can be reused to compute the Dogleg, thus reuse is + // set to true. + // + // If the user called StepIsInvalid, then there was a numerical + // problem with the step computed in the last call to ComputeStep, + // and the regularization used to do the Gauss-Newton solve is + // increased and a new solve should be done when ComputeStep is + // called again, thus reuse is set to false. + bool reuse_; + + // The dogleg type determines how the minimum of the local + // quadratic model is found. + DoglegType dogleg_type_; + + // If the type is SUBSPACE_DOGLEG, the two-dimensional + // model 1/2 x^T B x + g^T x has to be computed and stored. + bool subspace_is_one_dimensional_; + Matrix subspace_basis_; + Vector2d subspace_g_; + Matrix2d subspace_B_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DOGLEG_STRATEGY_H_ diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_finalizer.h b/extern/ceres/internal/ceres/dynamic_compressed_row_finalizer.h new file mode 100644 index 00000000000..a25a3083120 --- /dev/null +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_finalizer.h @@ -0,0 +1,51 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: richie.stebbing@gmail.com (Richard Stebbing) + +#ifndef CERES_INTERNAL_DYNAMIC_COMPRESED_ROW_FINALIZER_H_ +#define CERES_INTERNAL_DYNAMIC_COMPRESED_ROW_FINALIZER_H_ + +#include "ceres/casts.h" +#include "ceres/dynamic_compressed_row_sparse_matrix.h" + +namespace ceres { +namespace internal { + +struct DynamicCompressedRowJacobianFinalizer { + void operator()(SparseMatrix* base_jacobian, int num_parameters) { + DynamicCompressedRowSparseMatrix* jacobian = + down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian); + jacobian->Finalize(num_parameters); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DYNAMIC_COMPRESED_ROW_FINALISER_H_ diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc b/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc new file mode 100644 index 00000000000..fd5d89e350a --- /dev/null +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc @@ -0,0 +1,117 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: richie.stebbing@gmail.com (Richard Stebbing) + +#include "ceres/compressed_row_jacobian_writer.h" +#include "ceres/dynamic_compressed_row_jacobian_writer.h" +#include "ceres/casts.h" +#include "ceres/dynamic_compressed_row_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" + +namespace ceres { +namespace internal { + +using std::pair; +using std::vector; + +ScratchEvaluatePreparer* +DynamicCompressedRowJacobianWriter::CreateEvaluatePreparers(int num_threads) { + return ScratchEvaluatePreparer::Create(*program_, num_threads); +} + +SparseMatrix* DynamicCompressedRowJacobianWriter::CreateJacobian() const { + // Initialize `jacobian` with zero number of `max_num_nonzeros`. + const int num_residuals = program_->NumResiduals(); + const int num_effective_parameters = program_->NumEffectiveParameters(); + + DynamicCompressedRowSparseMatrix* jacobian = + new DynamicCompressedRowSparseMatrix(num_residuals, + num_effective_parameters, + 0); + + vector<int>* row_blocks = jacobian->mutable_row_blocks(); + for (int i = 0; i < jacobian->num_rows(); ++i) { + row_blocks->push_back(1); + } + + vector<int>* col_blocks = jacobian->mutable_col_blocks(); + for (int i = 0; i < jacobian->num_cols(); ++i) { + col_blocks->push_back(1); + } + + return jacobian; +} + +void DynamicCompressedRowJacobianWriter::Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian) { + DynamicCompressedRowSparseMatrix* jacobian = + down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian); + + // Get the `residual_block` of interest. + const ResidualBlock* residual_block = + program_->residual_blocks()[residual_id]; + const int num_residuals = residual_block->NumResiduals(); + + vector<pair<int, int> > evaluated_jacobian_blocks; + CompressedRowJacobianWriter::GetOrderedParameterBlocks( + program_, residual_id, &evaluated_jacobian_blocks); + + // `residual_offset` is the residual row in the global jacobian. + // Empty the jacobian rows. + jacobian->ClearRows(residual_offset, num_residuals); + + // Iterate over each parameter block. + for (int i = 0; i < evaluated_jacobian_blocks.size(); ++i) { + const ParameterBlock* parameter_block = + program_->parameter_blocks()[evaluated_jacobian_blocks[i].first]; + const int parameter_block_jacobian_index = + evaluated_jacobian_blocks[i].second; + const int parameter_block_size = parameter_block->LocalSize(); + + // For each parameter block only insert its non-zero entries. + for (int r = 0; r < num_residuals; ++r) { + for (int c = 0; c < parameter_block_size; ++c) { + const double& v = jacobians[parameter_block_jacobian_index][ + r * parameter_block_size + c]; + // Only insert non-zero entries. + if (v != 0.0) { + jacobian->InsertEntry( + residual_offset + r, parameter_block->delta_offset() + c, v); + } + } + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.h b/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.h new file mode 100644 index 00000000000..6e5ac38f07e --- /dev/null +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.h @@ -0,0 +1,83 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: richie.stebbing@gmail.com (Richard Stebbing) +// +// A jacobian writer that directly writes to dynamic compressed row sparse +// matrices. + +#ifndef CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_JACOBIAN_WRITER_H_ + +#include "ceres/evaluator.h" +#include "ceres/scratch_evaluate_preparer.h" + +namespace ceres { +namespace internal { + +class Program; +class SparseMatrix; + +class DynamicCompressedRowJacobianWriter { + public: + DynamicCompressedRowJacobianWriter(Evaluator::Options /* ignored */, + Program* program) + : program_(program) { + } + + // JacobianWriter interface. + + // The compressed row matrix has different layout than that assumed by + // the cost functions. The scratch space is therefore used to store + // the jacobians (including zeros) temporarily before only the non-zero + // entries are copied over to the larger jacobian in `Write`. + ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads); + + // Return a `DynamicCompressedRowSparseMatrix` which is filled by + // `Write`. Note that `Finalize` must be called to make the + // `CompressedRowSparseMatrix` interface valid. + SparseMatrix* CreateJacobian() const; + + // Write only the non-zero jacobian entries for a residual block + // (specified by `residual_id`) into `base_jacobian`, starting at the row + // specifed by `residual_offset`. + // + // This method is thread-safe over residual blocks (each `residual_id`). + void Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian); + + private: + Program* program_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_JACOBIAN_WRITER_H_ diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.cc b/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.cc new file mode 100644 index 00000000000..f020768ce10 --- /dev/null +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.cc @@ -0,0 +1,107 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: richie.stebbing@gmail.com (Richard Stebbing) + +#include <cstring> +#include "ceres/dynamic_compressed_row_sparse_matrix.h" + +namespace ceres { +namespace internal { + +DynamicCompressedRowSparseMatrix::DynamicCompressedRowSparseMatrix( + int num_rows, + int num_cols, + int initial_max_num_nonzeros) + : CompressedRowSparseMatrix(num_rows, + num_cols, + initial_max_num_nonzeros) { + dynamic_cols_.resize(num_rows); + dynamic_values_.resize(num_rows); + } + +void DynamicCompressedRowSparseMatrix::InsertEntry(int row, + int col, + const double& value) { + CHECK_GE(row, 0); + CHECK_LT(row, num_rows()); + CHECK_GE(col, 0); + CHECK_LT(col, num_cols()); + dynamic_cols_[row].push_back(col); + dynamic_values_[row].push_back(value); +} + +void DynamicCompressedRowSparseMatrix::ClearRows(int row_start, + int num_rows) { + for (int r = 0; r < num_rows; ++r) { + const int i = row_start + r; + CHECK_GE(i, 0); + CHECK_LT(i, this->num_rows()); + dynamic_cols_[i].resize(0); + dynamic_values_[i].resize(0); + } +} + +void DynamicCompressedRowSparseMatrix::Finalize(int num_additional_elements) { + // `num_additional_elements` is provided as an argument so that additional + // storage can be reserved when it is known by the finalizer. + CHECK_GE(num_additional_elements, 0); + + // Count the number of non-zeros and resize `cols_` and `values_`. + int num_jacobian_nonzeros = 0; + for (int i = 0; i < dynamic_cols_.size(); ++i) { + num_jacobian_nonzeros += dynamic_cols_[i].size(); + } + + SetMaxNumNonZeros(num_jacobian_nonzeros + num_additional_elements); + + // Flatten `dynamic_cols_` into `cols_` and `dynamic_values_` + // into `values_`. + int index_into_values_and_cols = 0; + for (int i = 0; i < num_rows(); ++i) { + mutable_rows()[i] = index_into_values_and_cols; + const int num_nonzero_columns = dynamic_cols_[i].size(); + if (num_nonzero_columns > 0) { + memcpy(mutable_cols() + index_into_values_and_cols, + &dynamic_cols_[i][0], + dynamic_cols_[i].size() * sizeof(dynamic_cols_[0][0])); + memcpy(mutable_values() + index_into_values_and_cols, + &dynamic_values_[i][0], + dynamic_values_[i].size() * sizeof(dynamic_values_[0][0])); + index_into_values_and_cols += dynamic_cols_[i].size(); + } + } + mutable_rows()[num_rows()] = index_into_values_and_cols; + + CHECK_EQ(index_into_values_and_cols, num_jacobian_nonzeros) + << "Ceres bug: final index into values_ and cols_ should be equal to " + << "the number of jacobian nonzeros. Please contact the developers!"; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.h b/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.h new file mode 100644 index 00000000000..cab860bddbd --- /dev/null +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.h @@ -0,0 +1,101 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: richie.stebbing@gmail.com (Richard Stebbing) +// +// A compressed row sparse matrix that provides an extended interface to +// allow dynamic insertion of entries. This is provided for the use case +// where the sparsity structure and number of non-zero entries is dynamic. +// This flexibility is achieved by using an (internal) scratch space that +// allows independent insertion of entries into each row (thread-safe). +// Once insertion is complete, the `Finalize` method must be called to ensure +// that the underlying `CompressedRowSparseMatrix` is consistent. +// +// This should only be used if you really do need a dynamic sparsity pattern. + +#ifndef CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_SPARSE_MATRIX_H_ + +#include <vector> + +#include "ceres/compressed_row_sparse_matrix.h" + +namespace ceres { +namespace internal { + +class DynamicCompressedRowSparseMatrix : public CompressedRowSparseMatrix { + public: + // Set the number of rows and columns for the underlyig + // `CompressedRowSparseMatrix` and set the initial number of maximum non-zero + // entries. Note that following the insertion of entries, when `Finalize` + // is called the number of non-zeros is determined and all internal + // structures are adjusted as required. If you know the upper limit on the + // number of non-zeros, then passing this value here can prevent future + // memory reallocations which may improve performance. Otherwise, if no + // upper limit is available a value of 0 is sufficient. + // + // Typical usage of this class is to define a new instance with a given + // number of rows, columns and maximum number of non-zero elements + // (if available). Next, entries are inserted at row and column positions + // using `InsertEntry`. Finally, once all elements have been inserted, + // `Finalize` must be called to make the underlying + // `CompressedRowSparseMatrix` consistent. + DynamicCompressedRowSparseMatrix(int num_rows, + int num_cols, + int initial_max_num_nonzeros); + + // Insert an entry at a given row and column position. This method is + // thread-safe across rows i.e. different threads can insert values + // simultaneously into different rows. It should be emphasised that this + // method always inserts a new entry and does not check for existing + // entries at the specified row and column position. Duplicate entries + // for a given row and column position will result in undefined + // behavior. + void InsertEntry(int row, int col, const double& value); + + // Clear all entries for rows, starting from row index `row_start` + // and proceeding for `num_rows`. + void ClearRows(int row_start, int num_rows); + + // Make the underlying internal `CompressedRowSparseMatrix` data structures + // consistent. Additional space for non-zero entries in the + // `CompressedRowSparseMatrix` can be reserved by specifying + // `num_additional_elements`. This is useful when it is known that rows will + // be appended to the `CompressedRowSparseMatrix` (e.g. appending a diagonal + // matrix to the jacobian) as it prevents need for future reallocation. + void Finalize(int num_additional_elements); + + private: + std::vector<std::vector<int> > dynamic_cols_; + std::vector<std::vector<double> > dynamic_values_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_SPARSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/evaluator.cc b/extern/ceres/internal/ceres/evaluator.cc new file mode 100644 index 00000000000..baba9afa11b --- /dev/null +++ b/extern/ceres/internal/ceres/evaluator.cc @@ -0,0 +1,86 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include <vector> +#include "ceres/block_evaluate_preparer.h" +#include "ceres/block_jacobian_writer.h" +#include "ceres/compressed_row_jacobian_writer.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/crs_matrix.h" +#include "ceres/dense_jacobian_writer.h" +#include "ceres/dynamic_compressed_row_finalizer.h" +#include "ceres/dynamic_compressed_row_jacobian_writer.h" +#include "ceres/evaluator.h" +#include "ceres/internal/port.h" +#include "ceres/program_evaluator.h" +#include "ceres/scratch_evaluate_preparer.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +Evaluator::~Evaluator() {} + +Evaluator* Evaluator::Create(const Evaluator::Options& options, + Program* program, + std::string* error) { + switch (options.linear_solver_type) { + case DENSE_QR: + case DENSE_NORMAL_CHOLESKY: + return new ProgramEvaluator<ScratchEvaluatePreparer, + DenseJacobianWriter>(options, + program); + case DENSE_SCHUR: + case SPARSE_SCHUR: + case ITERATIVE_SCHUR: + case CGNR: + return new ProgramEvaluator<BlockEvaluatePreparer, + BlockJacobianWriter>(options, + program); + case SPARSE_NORMAL_CHOLESKY: + if (options.dynamic_sparsity) { + return new ProgramEvaluator<ScratchEvaluatePreparer, + DynamicCompressedRowJacobianWriter, + DynamicCompressedRowJacobianFinalizer>( + options, program); + } else { + return new ProgramEvaluator<ScratchEvaluatePreparer, + CompressedRowJacobianWriter>(options, + program); + } + + default: + *error = "Invalid Linear Solver Type. Unable to create evaluator."; + return NULL; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/evaluator.h b/extern/ceres/internal/ceres/evaluator.h new file mode 100644 index 00000000000..fea307919d0 --- /dev/null +++ b/extern/ceres/internal/ceres/evaluator.h @@ -0,0 +1,205 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_EVALUATOR_H_ +#define CERES_INTERNAL_EVALUATOR_H_ + +#include <map> +#include <string> +#include <vector> + +#include "ceres/execution_summary.h" +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { + +struct CRSMatrix; + +namespace internal { + +class Program; +class SparseMatrix; + +// The Evaluator interface offers a way to interact with a least squares cost +// function that is useful for an optimizer that wants to minimize the least +// squares objective. This insulates the optimizer from issues like Jacobian +// storage, parameterization, etc. +class Evaluator { + public: + virtual ~Evaluator(); + + struct Options { + Options() + : num_threads(1), + num_eliminate_blocks(-1), + linear_solver_type(DENSE_QR), + dynamic_sparsity(false) {} + + int num_threads; + int num_eliminate_blocks; + LinearSolverType linear_solver_type; + bool dynamic_sparsity; + }; + + static Evaluator* Create(const Options& options, + Program* program, + std::string* error); + + // This is used for computing the cost, residual and Jacobian for + // returning to the user. For actually solving the optimization + // problem, the optimization algorithm uses the ProgramEvaluator + // objects directly. + // + // The residual, gradients and jacobian pointers can be NULL, in + // which case they will not be evaluated. cost cannot be NULL. + // + // The parallelism of the evaluator is controlled by num_threads; it + // should be at least 1. + // + // Note: That this function does not take a parameter vector as + // input. The parameter blocks are evaluated on the values contained + // in the arrays pointed to by their user_state pointers. + // + // Also worth noting is that this function mutates program by + // calling Program::SetParameterOffsetsAndIndex() on it so that an + // evaluator object can be constructed. + static bool Evaluate(Program* program, + int num_threads, + double* cost, + std::vector<double>* residuals, + std::vector<double>* gradient, + CRSMatrix* jacobian); + + // Build and return a sparse matrix for storing and working with the Jacobian + // of the objective function. The jacobian has dimensions + // NumEffectiveParameters() by NumParameters(), and is typically extremely + // sparse. Since the sparsity pattern of the Jacobian remains constant over + // the lifetime of the optimization problem, this method is used to + // instantiate a SparseMatrix object with the appropriate sparsity structure + // (which can be an expensive operation) and then reused by the optimization + // algorithm and the various linear solvers. + // + // It is expected that the classes implementing this interface will be aware + // of their client's requirements for the kind of sparse matrix storage and + // layout that is needed for an efficient implementation. For example + // CompressedRowOptimizationProblem creates a compressed row representation of + // the jacobian for use with CHOLMOD, where as BlockOptimizationProblem + // creates a BlockSparseMatrix representation of the jacobian for use in the + // Schur complement based methods. + virtual SparseMatrix* CreateJacobian() const = 0; + + + // Options struct to control Evaluator::Evaluate; + struct EvaluateOptions { + EvaluateOptions() + : apply_loss_function(true) { + } + + // If false, the loss function correction is not applied to the + // residual blocks. + bool apply_loss_function; + }; + + // Evaluate the cost function for the given state. Returns the cost, + // residuals, and jacobian in the corresponding arguments. Both residuals and + // jacobian are optional; to avoid computing them, pass NULL. + // + // If non-NULL, the Jacobian must have a suitable sparsity pattern; only the + // values array of the jacobian is modified. + // + // state is an array of size NumParameters(), cost is a pointer to a single + // double, and residuals is an array of doubles of size NumResiduals(). + virtual bool Evaluate(const EvaluateOptions& evaluate_options, + const double* state, + double* cost, + double* residuals, + double* gradient, + SparseMatrix* jacobian) = 0; + + // Variant of Evaluator::Evaluate where the user wishes to use the + // default EvaluateOptions struct. This is mostly here as a + // convenience method. + bool Evaluate(const double* state, + double* cost, + double* residuals, + double* gradient, + SparseMatrix* jacobian) { + return Evaluate(EvaluateOptions(), + state, + cost, + residuals, + gradient, + jacobian); + } + + // Make a change delta (of size NumEffectiveParameters()) to state (of size + // NumParameters()) and store the result in state_plus_delta. + // + // In the case that there are no parameterizations used, this is equivalent to + // + // state_plus_delta[i] = state[i] + delta[i] ; + // + // however, the mapping is more complicated in the case of parameterizations + // like quaternions. This is the same as the "Plus()" operation in + // local_parameterization.h, but operating over the entire state vector for a + // problem. + virtual bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const = 0; + + // The number of parameters in the optimization problem. + virtual int NumParameters() const = 0; + + // This is the effective number of parameters that the optimizer may adjust. + // This applies when there are parameterizations on some of the parameters. + virtual int NumEffectiveParameters() const = 0; + + // The number of residuals in the optimization problem. + virtual int NumResiduals() const = 0; + + // The following two methods return copies instead of references so + // that the base class implementation does not have to worry about + // life time issues. Further, these calls are not expected to be + // frequent or performance sensitive. + virtual std::map<std::string, int> CallStatistics() const { + return std::map<std::string, int>(); + } + + virtual std::map<std::string, double> TimeStatistics() const { + return std::map<std::string, double>(); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_EVALUATOR_H_ diff --git a/extern/ceres/internal/ceres/execution_summary.h b/extern/ceres/internal/ceres/execution_summary.h new file mode 100644 index 00000000000..aa9929d8974 --- /dev/null +++ b/extern/ceres/internal/ceres/execution_summary.h @@ -0,0 +1,90 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_EXECUTION_SUMMARY_H_ +#define CERES_INTERNAL_EXECUTION_SUMMARY_H_ + +#include <map> +#include <string> + +#include "ceres/internal/port.h" +#include "ceres/wall_time.h" +#include "ceres/mutex.h" + +namespace ceres { +namespace internal { + +// Struct used by various objects to report statistics and other +// information about their execution. e.g., ExecutionSummary::times +// can be used for reporting times associated with various activities. +class ExecutionSummary { + public: + void IncrementTimeBy(const std::string& name, const double value) { + CeresMutexLock l(×_mutex_); + times_[name] += value; + } + + void IncrementCall(const std::string& name) { + CeresMutexLock l(&calls_mutex_); + calls_[name] += 1; + } + + const std::map<std::string, double>& times() const { return times_; } + const std::map<std::string, int>& calls() const { return calls_; } + + private: + Mutex times_mutex_; + std::map<std::string, double> times_; + + Mutex calls_mutex_; + std::map<std::string, int> calls_; +}; + +class ScopedExecutionTimer { + public: + ScopedExecutionTimer(const std::string& name, ExecutionSummary* summary) + : start_time_(WallTimeInSeconds()), + name_(name), + summary_(summary) {} + + ~ScopedExecutionTimer() { + summary_->IncrementTimeBy(name_, WallTimeInSeconds() - start_time_); + } + + private: + const double start_time_; + const std::string name_; + ExecutionSummary* summary_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_EXECUTION_SUMMARY_H_ diff --git a/extern/ceres/internal/ceres/file.cc b/extern/ceres/internal/ceres/file.cc new file mode 100644 index 00000000000..c95a44d2c38 --- /dev/null +++ b/extern/ceres/internal/ceres/file.cc @@ -0,0 +1,95 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Really simple file IO. + +#include "ceres/file.h" + +#include <cstdio> +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::string; + +void WriteStringToFileOrDie(const string &data, const string &filename) { + FILE* file_descriptor = fopen(filename.c_str(), "wb"); + if (!file_descriptor) { + LOG(FATAL) << "Couldn't write to file: " << filename; + } + fwrite(data.c_str(), 1, data.size(), file_descriptor); + fclose(file_descriptor); +} + +void ReadFileToStringOrDie(const string &filename, string *data) { + FILE* file_descriptor = fopen(filename.c_str(), "r"); + + if (!file_descriptor) { + LOG(FATAL) << "Couldn't read file: " << filename; + } + + // Resize the input buffer appropriately. + fseek(file_descriptor, 0L, SEEK_END); + int num_bytes = ftell(file_descriptor); + data->resize(num_bytes); + + // Read the data. + fseek(file_descriptor, 0L, SEEK_SET); + int num_read = fread(&((*data)[0]), + sizeof((*data)[0]), + num_bytes, + file_descriptor); + if (num_read != num_bytes) { + LOG(FATAL) << "Couldn't read all of " << filename + << "expected bytes: " << num_bytes * sizeof((*data)[0]) + << "actual bytes: " << num_read; + } + fclose(file_descriptor); +} + +string JoinPath(const string& dirname, const string& basename) { +#ifdef _WIN32 + static const char separator = '\\'; +#else + static const char separator = '/'; +#endif // _WIN32 + + if ((!basename.empty() && basename[0] == separator) || dirname.empty()) { + return basename; + } else if (dirname[dirname.size() - 1] == separator) { + return dirname + basename; + } else { + return dirname + string(&separator, 1) + basename; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/file.h b/extern/ceres/internal/ceres/file.h new file mode 100644 index 00000000000..219b459b919 --- /dev/null +++ b/extern/ceres/internal/ceres/file.h @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Simple file IO support. This is a portability shim. + +#ifndef CERES_INTERNAL_FILE_H_ +#define CERES_INTERNAL_FILE_H_ + +#include <string> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +void WriteStringToFileOrDie(const std::string &data, + const std::string &filename); +void ReadFileToStringOrDie(const std::string &filename, std::string *data); + +// Join two path components, adding a slash if necessary. If basename is an +// absolute path then JoinPath ignores dirname and simply returns basename. +std::string JoinPath(const std::string& dirname, const std::string& basename); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_FILE_H_ diff --git a/extern/ceres/internal/ceres/generate_eliminator_specialization.py b/extern/ceres/internal/ceres/generate_eliminator_specialization.py new file mode 100644 index 00000000000..e89e7a48c98 --- /dev/null +++ b/extern/ceres/internal/ceres/generate_eliminator_specialization.py @@ -0,0 +1,231 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2015 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# 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) +# +# Script for explicitly generating template specialization of the +# SchurEliminator class. It is a rather large class +# and the number of explicit instantiations is also large. Explicitly +# generating these instantiations in separate .cc files breaks the +# compilation into separate compilation unit rather than one large cc +# file which takes 2+GB of RAM to compile. +# +# This script creates two sets of files. +# +# 1. schur_eliminator_x_x_x.cc +# where, the x indicates the template parameters and +# +# 2. schur_eliminator.cc +# +# that contains a factory function for instantiating these classes +# based on runtime parameters. +# +# The list of tuples, specializations indicates the set of +# specializations that is generated. + +# Set of template specializations to generate +SPECIALIZATIONS = [(2, 2, 2), + (2, 2, 3), + (2, 2, 4), + (2, 2, "Eigen::Dynamic"), + (2, 3, 3), + (2, 3, 4), + (2, 3, 6), + (2, 3, 9), + (2, 3, "Eigen::Dynamic"), + (2, 4, 3), + (2, 4, 4), + (2, 4, 8), + (2, 4, 9), + (2, 4, "Eigen::Dynamic"), + (2, "Eigen::Dynamic", "Eigen::Dynamic"), + (4, 4, 2), + (4, 4, 3), + (4, 4, 4), + (4, 4, "Eigen::Dynamic"), + ("Eigen::Dynamic", "Eigen::Dynamic", "Eigen::Dynamic")] +HEADER = """// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. +""" + +DYNAMIC_FILE = """ + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<%s, %s, %s>; + +} // namespace internal +} // namespace ceres +""" + +SPECIALIZATION_FILE = """ +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<%s, %s, %s>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION +""" + +FACTORY_FILE_HEADER = """ +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +SchurEliminatorBase* +SchurEliminatorBase::Create(const LinearSolver::Options& options) { +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION +""" + +FACTORY_CONDITIONAL = """ if ((options.row_block_size == %s) && + (options.e_block_size == %s) && + (options.f_block_size == %s)) { + return new SchurEliminator<%s, %s, %s>(options); + } +""" + +FACTORY_FOOTER = """ +#endif + VLOG(1) << "Template specializations not found for <" + << options.row_block_size << "," + << options.e_block_size << "," + << options.f_block_size << ">"; + return new SchurEliminator<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>(options); +} + +} // namespace internal +} // namespace ceres +""" + + +def SuffixForSize(size): + if size == "Eigen::Dynamic": + return "d" + return str(size) + + +def SpecializationFilename(prefix, row_block_size, e_block_size, f_block_size): + return "_".join([prefix] + map(SuffixForSize, (row_block_size, + e_block_size, + f_block_size))) + + +def Specialize(): + """ + Generate specialization code and the conditionals to instantiate it. + """ + f = open("schur_eliminator.cc", "w") + f.write(HEADER) + f.write(FACTORY_FILE_HEADER) + + for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS: + output = SpecializationFilename("generated/schur_eliminator", + row_block_size, + e_block_size, + f_block_size) + ".cc" + fptr = open(output, "w") + fptr.write(HEADER) + + template = SPECIALIZATION_FILE + if (row_block_size == "Eigen::Dynamic" and + e_block_size == "Eigen::Dynamic" and + f_block_size == "Eigen::Dynamic"): + template = DYNAMIC_FILE + + fptr.write(template % (row_block_size, e_block_size, f_block_size)) + fptr.close() + + f.write(FACTORY_CONDITIONAL % (row_block_size, + e_block_size, + f_block_size, + row_block_size, + e_block_size, + f_block_size)) + f.write(FACTORY_FOOTER) + f.close() + + +if __name__ == "__main__": + Specialize() diff --git a/extern/ceres/internal/ceres/generate_partitioned_matrix_view_specializations.py b/extern/ceres/internal/ceres/generate_partitioned_matrix_view_specializations.py new file mode 100644 index 00000000000..c4ac3cf2332 --- /dev/null +++ b/extern/ceres/internal/ceres/generate_partitioned_matrix_view_specializations.py @@ -0,0 +1,232 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2015 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# 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) +# +# Script for explicitly generating template specialization of the +# PartitionedMatrixView class. Explicitly generating these +# instantiations in separate .cc files breaks the compilation into +# separate compilation unit rather than one large cc file. +# +# This script creates two sets of files. +# +# 1. partitioned_matrix_view_x_x_x.cc +# where the x indicates the template parameters and +# +# 2. partitioned_matrix_view.cc +# +# that contains a factory function for instantiating these classes +# based on runtime parameters. +# +# The list of tuples, specializations indicates the set of +# specializations that is generated. + +# Set of template specializations to generate +SPECIALIZATIONS = [(2, 2, 2), + (2, 2, 3), + (2, 2, 4), + (2, 2, "Eigen::Dynamic"), + (2, 3, 3), + (2, 3, 4), + (2, 3, 6), + (2, 3, 9), + (2, 3, "Eigen::Dynamic"), + (2, 4, 3), + (2, 4, 4), + (2, 4, 8), + (2, 4, 9), + (2, 4, "Eigen::Dynamic"), + (2, "Eigen::Dynamic", "Eigen::Dynamic"), + (4, 4, 2), + (4, 4, 3), + (4, 4, 4), + (4, 4, "Eigen::Dynamic"), + ("Eigen::Dynamic", "Eigen::Dynamic", "Eigen::Dynamic")] +HEADER = """// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. +""" + +DYNAMIC_FILE = """ + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<%s, %s, %s>; + +} // namespace internal +} // namespace ceres +""" + +SPECIALIZATION_FILE = """ +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<%s, %s, %s>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION +""" + +FACTORY_FILE_HEADER = """ +#include "ceres/linear_solver.h" +#include "ceres/partitioned_matrix_view.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +PartitionedMatrixViewBase* +PartitionedMatrixViewBase::Create(const LinearSolver::Options& options, + const BlockSparseMatrix& matrix) { +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION +""" + +FACTORY_CONDITIONAL = """ if ((options.row_block_size == %s) && + (options.e_block_size == %s) && + (options.f_block_size == %s)) { + return new PartitionedMatrixView<%s, %s, %s>( + matrix, options.elimination_groups[0]); + } +""" + +FACTORY_FOOTER = """ +#endif + VLOG(1) << "Template specializations not found for <" + << options.row_block_size << "," + << options.e_block_size << "," + << options.f_block_size << ">"; + return new PartitionedMatrixView<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>( + matrix, options.elimination_groups[0]); +}; + +} // namespace internal +} // namespace ceres +""" + + +def SuffixForSize(size): + if size == "Eigen::Dynamic": + return "d" + return str(size) + + +def SpecializationFilename(prefix, row_block_size, e_block_size, f_block_size): + return "_".join([prefix] + map(SuffixForSize, (row_block_size, + e_block_size, + f_block_size))) + + +def Specialize(): + """ + Generate specialization code and the conditionals to instantiate it. + """ + f = open("partitioned_matrix_view.cc", "w") + f.write(HEADER) + f.write(FACTORY_FILE_HEADER) + + for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS: + output = SpecializationFilename("generated/partitioned_matrix_view", + row_block_size, + e_block_size, + f_block_size) + ".cc" + fptr = open(output, "w") + fptr.write(HEADER) + + template = SPECIALIZATION_FILE + if (row_block_size == "Eigen::Dynamic" and + e_block_size == "Eigen::Dynamic" and + f_block_size == "Eigen::Dynamic"): + template = DYNAMIC_FILE + + fptr.write(template % (row_block_size, e_block_size, f_block_size)) + fptr.close() + + f.write(FACTORY_CONDITIONAL % (row_block_size, + e_block_size, + f_block_size, + row_block_size, + e_block_size, + f_block_size)) + f.write(FACTORY_FOOTER) + f.close() + + +if __name__ == "__main__": + Specialize() diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc new file mode 100644 index 00000000000..500115b9897 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 2, 2>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc new file mode 100644 index 00000000000..1384cb619e3 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 2, 3>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc new file mode 100644 index 00000000000..030035ec97b --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 2, 4>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc new file mode 100644 index 00000000000..c9501b50170 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 2, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc new file mode 100644 index 00000000000..c2639bff69e --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 3, 3>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc new file mode 100644 index 00000000000..693e43959c1 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 3, 4>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_6.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_6.cc new file mode 100644 index 00000000000..7b9368ffefd --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_6.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 3, 6>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc new file mode 100644 index 00000000000..e72c5f6937a --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 3, 9>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc new file mode 100644 index 00000000000..c1f410eb64c --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 3, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc new file mode 100644 index 00000000000..7292c333d5d --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 4, 3>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc new file mode 100644 index 00000000000..891d65a8646 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 4, 4>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc new file mode 100644 index 00000000000..395f6bd4c13 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 4, 8>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc new file mode 100644 index 00000000000..88952b10e34 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 4, 9>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc new file mode 100644 index 00000000000..7733e1993eb --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 4, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc new file mode 100644 index 00000000000..117a0cdb8c1 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, Eigen::Dynamic, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc new file mode 100644 index 00000000000..a620bb70dba --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<4, 4, 2>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc new file mode 100644 index 00000000000..2978630832c --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<4, 4, 3>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc new file mode 100644 index 00000000000..bcd03b02e3a --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<4, 4, 4>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc new file mode 100644 index 00000000000..6b541ecf0d9 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<4, 4, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_d_d_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_d_d_d.cc new file mode 100644 index 00000000000..85111e722c4 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_d_d_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc new file mode 100644 index 00000000000..ac07a3f229e --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 2>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc new file mode 100644 index 00000000000..0ec09553f9e --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 3>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc new file mode 100644 index 00000000000..74a42cc4a16 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 4>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc new file mode 100644 index 00000000000..5ce757fda5d --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc new file mode 100644 index 00000000000..2e7ae28b4ea --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 3>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc new file mode 100644 index 00000000000..443207070cf --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 4>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_6.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_6.cc new file mode 100644 index 00000000000..ac2f358b383 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_6.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 6>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc new file mode 100644 index 00000000000..930ab440fa5 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 9>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc new file mode 100644 index 00000000000..486c53d36f4 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc new file mode 100644 index 00000000000..6f247a7b832 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, 3>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc new file mode 100644 index 00000000000..c44cd045263 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, 4>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_8.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_8.cc new file mode 100644 index 00000000000..c9a0d5fc729 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_8.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, 8>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_9.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_9.cc new file mode 100644 index 00000000000..b0455b0bca0 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_9.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, 9>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc new file mode 100644 index 00000000000..3234380f23c --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_d_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_d_d.cc new file mode 100644 index 00000000000..311f8556932 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_d_d.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, Eigen::Dynamic, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc new file mode 100644 index 00000000000..bc40bd55296 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 2>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc new file mode 100644 index 00000000000..cca88c802b0 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 3>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc new file mode 100644 index 00000000000..33c94a907b9 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 4>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc new file mode 100644 index 00000000000..1a1866f93a8 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc new file mode 100644 index 00000000000..6b18ef8c863 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>; + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/gradient_checking_cost_function.cc b/extern/ceres/internal/ceres/gradient_checking_cost_function.cc new file mode 100644 index 00000000000..580fd260e15 --- /dev/null +++ b/extern/ceres/internal/ceres/gradient_checking_cost_function.cc @@ -0,0 +1,337 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/gradient_checking_cost_function.h" + +#include <algorithm> +#include <cmath> +#include <numeric> +#include <string> +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/parameter_block.h" +#include "ceres/problem.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/dynamic_numeric_diff_cost_function.h" +#include "ceres/stringprintf.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::abs; +using std::max; +using std::string; +using std::vector; + +namespace { + +// True if x and y have an absolute relative difference less than +// relative_precision and false otherwise. Stores the relative and absolute +// difference in relative/absolute_error if non-NULL. +bool IsClose(double x, double y, double relative_precision, + double *relative_error, + double *absolute_error) { + double local_absolute_error; + double local_relative_error; + if (!absolute_error) { + absolute_error = &local_absolute_error; + } + if (!relative_error) { + relative_error = &local_relative_error; + } + *absolute_error = abs(x - y); + *relative_error = *absolute_error / max(abs(x), abs(y)); + if (x == 0 || y == 0) { + // If x or y is exactly zero, then relative difference doesn't have any + // meaning. Take the absolute difference instead. + *relative_error = *absolute_error; + } + return abs(*relative_error) < abs(relative_precision); +} + +class GradientCheckingCostFunction : public CostFunction { + public: + GradientCheckingCostFunction(const CostFunction* function, + const NumericDiffOptions& options, + double relative_precision, + const string& extra_info) + : function_(function), + relative_precision_(relative_precision), + extra_info_(extra_info) { + DynamicNumericDiffCostFunction<CostFunction, CENTRAL>* + finite_diff_cost_function = + new DynamicNumericDiffCostFunction<CostFunction, CENTRAL>( + function, + DO_NOT_TAKE_OWNERSHIP, + options); + + const vector<int32>& parameter_block_sizes = + function->parameter_block_sizes(); + for (int i = 0; i < parameter_block_sizes.size(); ++i) { + finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]); + } + *mutable_parameter_block_sizes() = parameter_block_sizes; + set_num_residuals(function->num_residuals()); + finite_diff_cost_function->SetNumResiduals(num_residuals()); + finite_diff_cost_function_.reset(finite_diff_cost_function); + } + + virtual ~GradientCheckingCostFunction() { } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + if (!jacobians) { + // Nothing to check in this case; just forward. + return function_->Evaluate(parameters, residuals, NULL); + } + + int num_residuals = function_->num_residuals(); + + // Make space for the jacobians of the two methods. + const vector<int32>& block_sizes = function_->parameter_block_sizes(); + vector<Matrix> term_jacobians(block_sizes.size()); + vector<Matrix> finite_difference_jacobians(block_sizes.size()); + vector<double*> term_jacobian_pointers(block_sizes.size()); + vector<double*> finite_difference_jacobian_pointers(block_sizes.size()); + for (int i = 0; i < block_sizes.size(); i++) { + term_jacobians[i].resize(num_residuals, block_sizes[i]); + term_jacobian_pointers[i] = term_jacobians[i].data(); + finite_difference_jacobians[i].resize(num_residuals, block_sizes[i]); + finite_difference_jacobian_pointers[i] = + finite_difference_jacobians[i].data(); + } + + // Evaluate the derivative using the user supplied code. + if (!function_->Evaluate(parameters, + residuals, + &term_jacobian_pointers[0])) { + LOG(WARNING) << "Function evaluation failed."; + return false; + } + + // Evaluate the derivative using numeric derivatives. + finite_diff_cost_function_->Evaluate( + parameters, + residuals, + &finite_difference_jacobian_pointers[0]); + + // See if any elements have relative error larger than the threshold. + int num_bad_jacobian_components = 0; + double worst_relative_error = 0; + + // Accumulate the error message for all the jacobians, since it won't get + // output if there are no bad jacobian components. + string m; + for (int k = 0; k < block_sizes.size(); k++) { + // Copy the original jacobian blocks into the jacobians array. + if (jacobians[k] != NULL) { + MatrixRef(jacobians[k], + term_jacobians[k].rows(), + term_jacobians[k].cols()) = term_jacobians[k]; + } + + StringAppendF(&m, + "========== " + "Jacobian for " "block %d: (%ld by %ld)) " + "==========\n", + k, + static_cast<long>(term_jacobians[k].rows()), + static_cast<long>(term_jacobians[k].cols())); + // The funny spacing creates appropriately aligned column headers. + m += " block row col user dx/dy num diff dx/dy " + "abs error relative error parameter residual\n"; + + for (int i = 0; i < term_jacobians[k].rows(); i++) { + for (int j = 0; j < term_jacobians[k].cols(); j++) { + double term_jacobian = term_jacobians[k](i, j); + double finite_jacobian = finite_difference_jacobians[k](i, j); + double relative_error, absolute_error; + bool bad_jacobian_entry = + !IsClose(term_jacobian, + finite_jacobian, + relative_precision_, + &relative_error, + &absolute_error); + worst_relative_error = max(worst_relative_error, relative_error); + + StringAppendF(&m, "%6d %4d %4d %17g %17g %17g %17g %17g %17g", + k, i, j, + term_jacobian, finite_jacobian, + absolute_error, relative_error, + parameters[k][j], + residuals[i]); + + if (bad_jacobian_entry) { + num_bad_jacobian_components++; + StringAppendF( + &m, " ------ (%d,%d,%d) Relative error worse than %g", + k, i, j, relative_precision_); + } + m += "\n"; + } + } + } + + // Since there were some bad errors, dump comprehensive debug info. + if (num_bad_jacobian_components) { + string header = StringPrintf("Detected %d bad jacobian component(s). " + "Worst relative error was %g.\n", + num_bad_jacobian_components, + worst_relative_error); + if (!extra_info_.empty()) { + header += "Extra info for this residual: " + extra_info_ + "\n"; + } + LOG(WARNING) << "\n" << header << m; + } + return true; + } + + private: + const CostFunction* function_; + internal::scoped_ptr<CostFunction> finite_diff_cost_function_; + double relative_precision_; + string extra_info_; +}; + +} // namespace + +CostFunction *CreateGradientCheckingCostFunction( + const CostFunction *cost_function, + double relative_step_size, + double relative_precision, + const string& extra_info) { + NumericDiffOptions numeric_diff_options; + numeric_diff_options.relative_step_size = relative_step_size; + + return new GradientCheckingCostFunction(cost_function, + numeric_diff_options, + relative_precision, + extra_info); +} + +ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision) { + // We create new CostFunctions by wrapping the original CostFunction + // in a gradient checking CostFunction. So its okay for the + // ProblemImpl to take ownership of it and destroy it. The + // LossFunctions and LocalParameterizations are reused and since + // they are owned by problem_impl, gradient_checking_problem_impl + // should not take ownership of it. + Problem::Options gradient_checking_problem_options; + gradient_checking_problem_options.cost_function_ownership = TAKE_OWNERSHIP; + gradient_checking_problem_options.loss_function_ownership = + DO_NOT_TAKE_OWNERSHIP; + gradient_checking_problem_options.local_parameterization_ownership = + DO_NOT_TAKE_OWNERSHIP; + + ProblemImpl* gradient_checking_problem_impl = new ProblemImpl( + gradient_checking_problem_options); + + Program* program = problem_impl->mutable_program(); + + // For every ParameterBlock in problem_impl, create a new parameter + // block with the same local parameterization and constancy. + const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + gradient_checking_problem_impl->AddParameterBlock( + parameter_block->mutable_user_state(), + parameter_block->Size(), + parameter_block->mutable_local_parameterization()); + + if (parameter_block->IsConstant()) { + gradient_checking_problem_impl->SetParameterBlockConstant( + parameter_block->mutable_user_state()); + } + } + + // For every ResidualBlock in problem_impl, create a new + // ResidualBlock by wrapping its CostFunction inside a + // GradientCheckingCostFunction. + const vector<ResidualBlock*>& residual_blocks = program->residual_blocks(); + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + + // Build a human readable string which identifies the + // ResidualBlock. This is used by the GradientCheckingCostFunction + // when logging debugging information. + string extra_info = StringPrintf( + "Residual block id %d; depends on parameters [", i); + vector<double*> parameter_blocks; + for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + parameter_blocks.push_back(parameter_block->mutable_user_state()); + StringAppendF(&extra_info, "%p", parameter_block->mutable_user_state()); + extra_info += (j < residual_block->NumParameterBlocks() - 1) ? ", " : "]"; + } + + // Wrap the original CostFunction in a GradientCheckingCostFunction. + CostFunction* gradient_checking_cost_function = + CreateGradientCheckingCostFunction(residual_block->cost_function(), + relative_step_size, + relative_precision, + extra_info); + + // The const_cast is necessary because + // ProblemImpl::AddResidualBlock can potentially take ownership of + // the LossFunction, but in this case we are guaranteed that this + // will not be the case, so this const_cast is harmless. + gradient_checking_problem_impl->AddResidualBlock( + gradient_checking_cost_function, + const_cast<LossFunction*>(residual_block->loss_function()), + parameter_blocks); + } + + // Normally, when a problem is given to the solver, we guarantee + // that the state pointers for each parameter block point to the + // user provided data. Since we are creating this new problem from a + // problem given to us at an arbitrary stage of the solve, we cannot + // depend on this being the case, so we explicitly call + // SetParameterBlockStatePtrsToUserStatePtrs to ensure that this is + // the case. + gradient_checking_problem_impl + ->mutable_program() + ->SetParameterBlockStatePtrsToUserStatePtrs(); + + return gradient_checking_problem_impl; +} + + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/gradient_checking_cost_function.h b/extern/ceres/internal/ceres/gradient_checking_cost_function.h new file mode 100644 index 00000000000..cf92cb72bc5 --- /dev/null +++ b/extern/ceres/internal/ceres/gradient_checking_cost_function.h @@ -0,0 +1,85 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ +#define CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ + +#include <string> + +#include "ceres/cost_function.h" + +namespace ceres { +namespace internal { + +class ProblemImpl; + +// Creates a CostFunction that checks the jacobians that cost_function computes +// with finite differences. Bad results are logged; required precision is +// controlled by relative_precision and the numeric differentiation step size is +// controlled with relative_step_size. See solver.h for a better explanation of +// relative_step_size. Caller owns result. +// +// The condition enforced is that +// +// (J_actual(i, j) - J_numeric(i, j)) +// ------------------------------------ < relative_precision +// max(J_actual(i, j), J_numeric(i, j)) +// +// where J_actual(i, j) is the jacobian as computed by the supplied cost +// function (by the user) and J_numeric is the jacobian as computed by finite +// differences. +// +// Note: This is quite inefficient and is intended only for debugging. +CostFunction* CreateGradientCheckingCostFunction( + const CostFunction* cost_function, + double relative_step_size, + double relative_precision, + const std::string& extra_info); + +// Create a new ProblemImpl object from the input problem_impl, where +// each CostFunctions in problem_impl are wrapped inside a +// GradientCheckingCostFunctions. This gives us a ProblemImpl object +// which checks its derivatives against estimates from numeric +// differentiation everytime a ResidualBlock is evaluated. +// +// relative_step_size and relative_precision are parameters to control +// the numeric differentiation and the relative tolerance between the +// jacobian computed by the CostFunctions in problem_impl and +// jacobians obtained by numerically differentiating them. For more +// details see the documentation for +// CreateGradientCheckingCostFunction above. +ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ diff --git a/extern/ceres/internal/ceres/gradient_problem.cc b/extern/ceres/internal/ceres/gradient_problem.cc new file mode 100644 index 00000000000..4ebd3e60610 --- /dev/null +++ b/extern/ceres/internal/ceres/gradient_problem.cc @@ -0,0 +1,81 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/gradient_problem.h" +#include "ceres/local_parameterization.h" +#include "glog/logging.h" + +namespace ceres { + +GradientProblem::GradientProblem(FirstOrderFunction* function) + : function_(function), + parameterization_( + new IdentityParameterization(function_->NumParameters())), + scratch_(new double[function_->NumParameters()]) { +} + +GradientProblem::GradientProblem(FirstOrderFunction* function, + LocalParameterization* parameterization) + : function_(function), + parameterization_(parameterization), + scratch_(new double[function_->NumParameters()]) { + CHECK_EQ(function_->NumParameters(), parameterization_->GlobalSize()); +} + +int GradientProblem::NumParameters() const { + return function_->NumParameters(); +} + +int GradientProblem::NumLocalParameters() const { + return parameterization_->LocalSize(); +} + + +bool GradientProblem::Evaluate(const double* parameters, + double* cost, + double* gradient) const { + if (gradient == NULL) { + return function_->Evaluate(parameters, cost, NULL); + } + + return (function_->Evaluate(parameters, cost, scratch_.get()) && + parameterization_->MultiplyByJacobian(parameters, + 1, + scratch_.get(), + gradient)); +} + +bool GradientProblem::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + return parameterization_->Plus(x, delta, x_plus_delta); +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/gradient_problem_evaluator.h b/extern/ceres/internal/ceres/gradient_problem_evaluator.h new file mode 100644 index 00000000000..2c562544768 --- /dev/null +++ b/extern/ceres/internal/ceres/gradient_problem_evaluator.h @@ -0,0 +1,98 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_GRADIENT_PROBLEM_EVALUATOR_H_ +#define CERES_INTERNAL_GRADIENT_PROBLEM_EVALUATOR_H_ + +#include <map> +#include <string> + +#include "ceres/evaluator.h" +#include "ceres/execution_summary.h" +#include "ceres/gradient_problem.h" +#include "ceres/internal/port.h" +#include "ceres/wall_time.h" + +namespace ceres { +namespace internal { + +class GradientProblemEvaluator : public Evaluator { + public: + explicit GradientProblemEvaluator(const GradientProblem& problem) + : problem_(problem) {} + virtual ~GradientProblemEvaluator() {} + virtual SparseMatrix* CreateJacobian() const { return NULL; } + virtual bool Evaluate(const EvaluateOptions& evaluate_options, + const double* state, + double* cost, + double* residuals, + double* gradient, + SparseMatrix* jacobian) { + CHECK(jacobian == NULL); + ScopedExecutionTimer total_timer("Evaluator::Total", &execution_summary_); + ScopedExecutionTimer call_type_timer( + gradient == NULL ? "Evaluator::Cost" : "Evaluator::Gradient", + &execution_summary_); + return problem_.Evaluate(state, cost, gradient); + } + + virtual bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const { + return problem_.Plus(state, delta, state_plus_delta); + } + + virtual int NumParameters() const { + return problem_.NumParameters(); + } + + virtual int NumEffectiveParameters() const { + return problem_.NumLocalParameters(); + } + + virtual int NumResiduals() const { return 1; } + + virtual std::map<std::string, int> CallStatistics() const { + return execution_summary_.calls(); + } + + virtual std::map<std::string, double> TimeStatistics() const { + return execution_summary_.times(); + } + + private: + const GradientProblem& problem_; + ::ceres::internal::ExecutionSummary execution_summary_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRADIENT_PROBLEM_EVALUATOR_H_ diff --git a/extern/ceres/internal/ceres/gradient_problem_solver.cc b/extern/ceres/internal/ceres/gradient_problem_solver.cc new file mode 100644 index 00000000000..9a549c23dac --- /dev/null +++ b/extern/ceres/internal/ceres/gradient_problem_solver.cc @@ -0,0 +1,277 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/gradient_problem_solver.h" + +#include "ceres/callbacks.h" +#include "ceres/gradient_problem.h" +#include "ceres/gradient_problem_evaluator.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/map_util.h" +#include "ceres/minimizer.h" +#include "ceres/solver.h" +#include "ceres/solver_utils.h" +#include "ceres/stringprintf.h" +#include "ceres/types.h" +#include "ceres/wall_time.h" + +namespace ceres { +using internal::StringPrintf; +using internal::StringAppendF; +using std::string; + +namespace { + +Solver::Options GradientProblemSolverOptionsToSolverOptions( + const GradientProblemSolver::Options& options) { +#define COPY_OPTION(x) solver_options.x = options.x + + Solver::Options solver_options; + solver_options.minimizer_type = LINE_SEARCH; + COPY_OPTION(line_search_direction_type); + COPY_OPTION(line_search_type); + COPY_OPTION(nonlinear_conjugate_gradient_type); + COPY_OPTION(max_lbfgs_rank); + COPY_OPTION(use_approximate_eigenvalue_bfgs_scaling); + COPY_OPTION(line_search_interpolation_type); + COPY_OPTION(min_line_search_step_size); + COPY_OPTION(line_search_sufficient_function_decrease); + COPY_OPTION(max_line_search_step_contraction); + COPY_OPTION(min_line_search_step_contraction); + COPY_OPTION(max_num_line_search_step_size_iterations); + COPY_OPTION(max_num_line_search_direction_restarts); + COPY_OPTION(line_search_sufficient_curvature_decrease); + COPY_OPTION(max_line_search_step_expansion); + COPY_OPTION(max_num_iterations); + COPY_OPTION(max_solver_time_in_seconds); + COPY_OPTION(function_tolerance); + COPY_OPTION(gradient_tolerance); + COPY_OPTION(logging_type); + COPY_OPTION(minimizer_progress_to_stdout); + COPY_OPTION(callbacks); + return solver_options; +#undef COPY_OPTION +} + + +} // namespace + +GradientProblemSolver::~GradientProblemSolver() { +} + +void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options, + const GradientProblem& problem, + double* parameters_ptr, + GradientProblemSolver::Summary* summary) { + using internal::scoped_ptr; + using internal::WallTimeInSeconds; + using internal::Minimizer; + using internal::GradientProblemEvaluator; + using internal::LoggingCallback; + using internal::SetSummaryFinalCost; + + double start_time = WallTimeInSeconds(); + Solver::Options solver_options = + GradientProblemSolverOptionsToSolverOptions(options); + + *CHECK_NOTNULL(summary) = Summary(); + summary->num_parameters = problem.NumParameters(); + summary->num_local_parameters = problem.NumLocalParameters(); + summary->line_search_direction_type = options.line_search_direction_type; // NOLINT + summary->line_search_interpolation_type = options.line_search_interpolation_type; // NOLINT + summary->line_search_type = options.line_search_type; + summary->max_lbfgs_rank = options.max_lbfgs_rank; + summary->nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; // NOLINT + + // Check validity + if (!solver_options.IsValid(&summary->message)) { + LOG(ERROR) << "Terminating: " << summary->message; + return; + } + + // Assuming that the parameter blocks in the program have been + Minimizer::Options minimizer_options; + minimizer_options = Minimizer::Options(solver_options); + minimizer_options.evaluator.reset(new GradientProblemEvaluator(problem)); + + scoped_ptr<IterationCallback> logging_callback; + if (options.logging_type != SILENT) { + logging_callback.reset( + new LoggingCallback(LINE_SEARCH, options.minimizer_progress_to_stdout)); + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + logging_callback.get()); + } + + scoped_ptr<Minimizer> minimizer(Minimizer::Create(LINE_SEARCH)); + Vector solution(problem.NumParameters()); + VectorRef parameters(parameters_ptr, problem.NumParameters()); + solution = parameters; + + Solver::Summary solver_summary; + solver_summary.fixed_cost = 0.0; + solver_summary.preprocessor_time_in_seconds = 0.0; + solver_summary.postprocessor_time_in_seconds = 0.0; + solver_summary.line_search_polynomial_minimization_time_in_seconds = 0.0; + + minimizer->Minimize(minimizer_options, solution.data(), &solver_summary); + + summary->termination_type = solver_summary.termination_type; + summary->message = solver_summary.message; + summary->initial_cost = solver_summary.initial_cost; + summary->final_cost = solver_summary.final_cost; + summary->iterations = solver_summary.iterations; + summary->line_search_polynomial_minimization_time_in_seconds = + solver_summary.line_search_polynomial_minimization_time_in_seconds; + + if (summary->IsSolutionUsable()) { + parameters = solution; + SetSummaryFinalCost(summary); + } + + const std::map<string, double>& evaluator_time_statistics = + minimizer_options.evaluator->TimeStatistics(); + summary->cost_evaluation_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0); + summary->gradient_evaluation_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0); + + summary->total_time_in_seconds = WallTimeInSeconds() - start_time; +} + +// Invalid values for most fields, to ensure that we are not +// accidentally reporting default values. +GradientProblemSolver::Summary::Summary() + : termination_type(FAILURE), + message("ceres::GradientProblemSolve was not called."), + initial_cost(-1.0), + final_cost(-1.0), + total_time_in_seconds(-1.0), + cost_evaluation_time_in_seconds(-1.0), + gradient_evaluation_time_in_seconds(-1.0), + line_search_polynomial_minimization_time_in_seconds(-1.0), + num_parameters(-1), + num_local_parameters(-1), + line_search_direction_type(LBFGS), + line_search_type(ARMIJO), + line_search_interpolation_type(BISECTION), + nonlinear_conjugate_gradient_type(FLETCHER_REEVES), + max_lbfgs_rank(-1) { +} + +bool GradientProblemSolver::Summary::IsSolutionUsable() const { + return internal::IsSolutionUsable(*this); +} + +string GradientProblemSolver::Summary::BriefReport() const { + return StringPrintf("Ceres GradientProblemSolver Report: " + "Iterations: %d, " + "Initial cost: %e, " + "Final cost: %e, " + "Termination: %s", + static_cast<int>(iterations.size()), + initial_cost, + final_cost, + TerminationTypeToString(termination_type)); +} + +string GradientProblemSolver::Summary::FullReport() const { + using internal::VersionString; + + string report = string("\nSolver Summary (v " + VersionString() + ")\n\n"); + + StringAppendF(&report, "Parameters % 25d\n", num_parameters); + if (num_local_parameters != num_parameters) { + StringAppendF(&report, "Local parameters % 25d\n", + num_local_parameters); + } + + string line_search_direction_string; + if (line_search_direction_type == LBFGS) { + 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 { + line_search_direction_string = + LineSearchDirectionTypeToString(line_search_direction_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, "\nCost:\n"); + StringAppendF(&report, "Initial % 30e\n", initial_cost); + if (termination_type != FAILURE && + termination_type != USER_FAILURE) { + StringAppendF(&report, "Final % 30e\n", final_cost); + StringAppendF(&report, "Change % 30e\n", + initial_cost - final_cost); + } + + StringAppendF(&report, "\nMinimizer iterations % 16d\n", + static_cast<int>(iterations.size())); + + StringAppendF(&report, "\nTime (in seconds):\n"); + + StringAppendF(&report, "\n Cost evaluation %23.4f\n", + cost_evaluation_time_in_seconds); + StringAppendF(&report, " Gradient evaluation %23.4f\n", + gradient_evaluation_time_in_seconds); + StringAppendF(&report, " Polynomial minimization %17.4f\n", + line_search_polynomial_minimization_time_in_seconds); + + StringAppendF(&report, "Total %25.4f\n\n", + total_time_in_seconds); + + StringAppendF(&report, "Termination: %25s (%s)\n", + TerminationTypeToString(termination_type), message.c_str()); + return report; +} + +void Solve(const GradientProblemSolver::Options& options, + const GradientProblem& problem, + double* parameters, + GradientProblemSolver::Summary* summary) { + GradientProblemSolver solver; + solver.Solve(options, problem, parameters, summary); +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/graph.h b/extern/ceres/internal/ceres/graph.h new file mode 100644 index 00000000000..b96b67265cb --- /dev/null +++ b/extern/ceres/internal/ceres/graph.h @@ -0,0 +1,225 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_GRAPH_H_ +#define CERES_INTERNAL_GRAPH_H_ + +#include <limits> +#include <utility> +#include "ceres/integral_types.h" +#include "ceres/map_util.h" +#include "ceres/collections_port.h" +#include "ceres/internal/macros.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// A unweighted undirected graph templated over the vertex ids. Vertex +// should be hashable. +template <typename Vertex> +class Graph { + public: + Graph() {} + + // Add a vertex. + void AddVertex(const Vertex& vertex) { + if (vertices_.insert(vertex).second) { + edges_[vertex] = HashSet<Vertex>(); + } + } + + bool RemoveVertex(const Vertex& vertex) { + if (vertices_.find(vertex) == vertices_.end()) { + return false; + } + + vertices_.erase(vertex); + const HashSet<Vertex>& sinks = edges_[vertex]; + for (typename HashSet<Vertex>::const_iterator it = sinks.begin(); + it != sinks.end(); ++it) { + edges_[*it].erase(vertex); + } + + edges_.erase(vertex); + return true; + } + + // Add an edge between the vertex1 and vertex2. Calling AddEdge on a + // pair of vertices which do not exist in the graph yet will result + // in undefined behavior. + // + // It is legal to call this method repeatedly for the same set of + // vertices. + void AddEdge(const Vertex& vertex1, const Vertex& vertex2) { + DCHECK(vertices_.find(vertex1) != vertices_.end()); + DCHECK(vertices_.find(vertex2) != vertices_.end()); + + if (edges_[vertex1].insert(vertex2).second) { + edges_[vertex2].insert(vertex1); + } + } + + // Calling Neighbors on a vertex not in the graph will result in + // undefined behaviour. + const HashSet<Vertex>& Neighbors(const Vertex& vertex) const { + return FindOrDie(edges_, vertex); + } + + const HashSet<Vertex>& vertices() const { + return vertices_; + } + + private: + HashSet<Vertex> vertices_; + HashMap<Vertex, HashSet<Vertex> > edges_; + + CERES_DISALLOW_COPY_AND_ASSIGN(Graph); +}; + +// A weighted undirected graph templated over the vertex ids. Vertex +// should be hashable and comparable. +template <typename Vertex> +class WeightedGraph { + public: + WeightedGraph() {} + + // Add a weighted vertex. If the vertex already exists in the graph, + // its weight is set to the new weight. + void AddVertex(const Vertex& vertex, double weight) { + if (vertices_.find(vertex) == vertices_.end()) { + vertices_.insert(vertex); + edges_[vertex] = HashSet<Vertex>(); + } + vertex_weights_[vertex] = weight; + } + + // Uses weight = 1.0. If vertex already exists, its weight is set to + // 1.0. + void AddVertex(const Vertex& vertex) { + AddVertex(vertex, 1.0); + } + + bool RemoveVertex(const Vertex& vertex) { + if (vertices_.find(vertex) == vertices_.end()) { + return false; + } + + vertices_.erase(vertex); + vertex_weights_.erase(vertex); + const HashSet<Vertex>& sinks = edges_[vertex]; + for (typename HashSet<Vertex>::const_iterator it = sinks.begin(); + it != sinks.end(); ++it) { + if (vertex < *it) { + edge_weights_.erase(std::make_pair(vertex, *it)); + } else { + edge_weights_.erase(std::make_pair(*it, vertex)); + } + edges_[*it].erase(vertex); + } + + edges_.erase(vertex); + return true; + } + + // Add a weighted edge between the vertex1 and vertex2. Calling + // AddEdge on a pair of vertices which do not exist in the graph yet + // will result in undefined behavior. + // + // It is legal to call this method repeatedly for the same set of + // vertices. + void AddEdge(const Vertex& vertex1, const Vertex& vertex2, double weight) { + DCHECK(vertices_.find(vertex1) != vertices_.end()); + DCHECK(vertices_.find(vertex2) != vertices_.end()); + + if (edges_[vertex1].insert(vertex2).second) { + edges_[vertex2].insert(vertex1); + } + + if (vertex1 < vertex2) { + edge_weights_[std::make_pair(vertex1, vertex2)] = weight; + } else { + edge_weights_[std::make_pair(vertex2, vertex1)] = weight; + } + } + + // Uses weight = 1.0. + void AddEdge(const Vertex& vertex1, const Vertex& vertex2) { + AddEdge(vertex1, vertex2, 1.0); + } + + // Calling VertexWeight on a vertex not in the graph will result in + // undefined behavior. + double VertexWeight(const Vertex& vertex) const { + return FindOrDie(vertex_weights_, vertex); + } + + // Calling EdgeWeight on a pair of vertices where either one of the + // vertices is not present in the graph will result in undefined + // behaviour. If there is no edge connecting vertex1 and vertex2, + // the edge weight is zero. + double EdgeWeight(const Vertex& vertex1, const Vertex& vertex2) const { + if (vertex1 < vertex2) { + return FindWithDefault(edge_weights_, + std::make_pair(vertex1, vertex2), 0.0); + } else { + return FindWithDefault(edge_weights_, + std::make_pair(vertex2, vertex1), 0.0); + } + } + + // Calling Neighbors on a vertex not in the graph will result in + // undefined behaviour. + const HashSet<Vertex>& Neighbors(const Vertex& vertex) const { + return FindOrDie(edges_, vertex); + } + + const HashSet<Vertex>& vertices() const { + return vertices_; + } + + static double InvalidWeight() { + return std::numeric_limits<double>::quiet_NaN(); + } + + private: + HashSet<Vertex> vertices_; + HashMap<Vertex, double> vertex_weights_; + HashMap<Vertex, HashSet<Vertex> > edges_; + HashMap<std::pair<Vertex, Vertex>, double> edge_weights_; + + CERES_DISALLOW_COPY_AND_ASSIGN(WeightedGraph); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRAPH_H_ diff --git a/extern/ceres/internal/ceres/graph_algorithms.h b/extern/ceres/internal/ceres/graph_algorithms.h new file mode 100644 index 00000000000..d1d3f52cd22 --- /dev/null +++ b/extern/ceres/internal/ceres/graph_algorithms.h @@ -0,0 +1,364 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Various algorithms that operate on undirected graphs. + +#ifndef CERES_INTERNAL_GRAPH_ALGORITHMS_H_ +#define CERES_INTERNAL_GRAPH_ALGORITHMS_H_ + +#include <algorithm> +#include <vector> +#include <utility> +#include "ceres/collections_port.h" +#include "ceres/graph.h" +#include "ceres/wall_time.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Compare two vertices of a graph by their degrees, if the degrees +// are equal then order them by their ids. +template <typename Vertex> +class VertexTotalOrdering { + public: + explicit VertexTotalOrdering(const Graph<Vertex>& graph) + : graph_(graph) {} + + bool operator()(const Vertex& lhs, const Vertex& rhs) const { + if (graph_.Neighbors(lhs).size() == graph_.Neighbors(rhs).size()) { + return lhs < rhs; + } + return graph_.Neighbors(lhs).size() < graph_.Neighbors(rhs).size(); + } + + private: + const Graph<Vertex>& graph_; +}; + +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 +// independent set problem is NP-Hard, but there are effective +// approximation algorithms available. The implementation here uses a +// breadth first search that explores the vertices in order of +// increasing degree. The same idea is used by Saad & Li in "MIQR: A +// multilevel incomplete QR preconditioner for large sparse +// least-squares problems", SIMAX, 2007. +// +// Given a undirected graph G(V,E), the algorithm is a greedy BFS +// search where the vertices are explored in increasing order of their +// degree. The output vector ordering contains elements of S in +// increasing order of their degree, followed by elements of V - S in +// increasing order of degree. The return value of the function is the +// cardinality of S. +template <typename Vertex> +int IndependentSetOrdering(const Graph<Vertex>& graph, + std::vector<Vertex>* ordering) { + const HashSet<Vertex>& vertices = graph.vertices(); + const int num_vertices = vertices.size(); + + CHECK_NOTNULL(ordering); + ordering->clear(); + ordering->reserve(num_vertices); + + // Colors for labeling the graph during the BFS. + const char kWhite = 0; + const char kGrey = 1; + const char kBlack = 2; + + // Mark all vertices white. + HashMap<Vertex, char> vertex_color; + std::vector<Vertex> vertex_queue; + for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); + it != vertices.end(); + ++it) { + vertex_color[*it] = kWhite; + vertex_queue.push_back(*it); + } + + + std::sort(vertex_queue.begin(), vertex_queue.end(), + 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 std::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, + std::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; + + std::vector<Vertex> vertex_queue(*ordering); + + std::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) { + 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 std::vector<Vertex>::const_iterator it = vertex_queue.begin(); + it != vertex_queue.end(); + ++it) { + const Vertex vertex = *it; + DCHECK(vertex_color[vertex] != kWhite); + if (vertex_color[vertex] != kBlack) { + ordering->push_back(vertex); + } + } + + CHECK_EQ(ordering->size(), num_vertices); + return independent_set_size; +} + +// Find the connected component for a vertex implemented using the +// find and update operation for disjoint-set. Recursively traverse +// the disjoint set structure till you reach a vertex whose connected +// component has the same id as the vertex itself. Along the way +// update the connected components of all the vertices. This updating +// is what gives this data structure its efficiency. +template <typename Vertex> +Vertex FindConnectedComponent(const Vertex& vertex, + HashMap<Vertex, Vertex>* union_find) { + typename HashMap<Vertex, Vertex>::iterator it = union_find->find(vertex); + DCHECK(it != union_find->end()); + if (it->second != vertex) { + it->second = FindConnectedComponent(it->second, union_find); + } + + return it->second; +} + +// Compute a degree two constrained Maximum Spanning Tree/forest of +// the input graph. Caller owns the result. +// +// Finding degree 2 spanning tree of a graph is not always +// possible. For example a star graph, i.e. a graph with n-nodes +// where one node is connected to the other n-1 nodes does not have +// a any spanning trees of degree less than n-1.Even if such a tree +// exists, finding such a tree is NP-Hard. + +// We get around both of these problems by using a greedy, degree +// constrained variant of Kruskal's algorithm. We start with a graph +// G_T with the same vertex set V as the input graph G(V,E) but an +// empty edge set. We then iterate over the edges of G in decreasing +// order of weight, adding them to G_T if doing so does not create a +// cycle in G_T} and the degree of all the vertices in G_T remains +// bounded by two. This O(|E|) algorithm results in a degree-2 +// spanning forest, or a collection of linear paths that span the +// graph G. +template <typename Vertex> +WeightedGraph<Vertex>* +Degree2MaximumSpanningForest(const WeightedGraph<Vertex>& graph) { + // Array of edges sorted in decreasing order of their weights. + std::vector<std::pair<double, std::pair<Vertex, Vertex> > > weighted_edges; + WeightedGraph<Vertex>* forest = new WeightedGraph<Vertex>(); + + // Disjoint-set to keep track of the connected components in the + // maximum spanning tree. + HashMap<Vertex, Vertex> disjoint_set; + + // Sort of the edges in the graph in decreasing order of their + // weight. Also add the vertices of the graph to the Maximum + // Spanning Tree graph and set each vertex to be its own connected + // component in the disjoint_set structure. + const HashSet<Vertex>& vertices = graph.vertices(); + for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); + it != vertices.end(); + ++it) { + const Vertex vertex1 = *it; + forest->AddVertex(vertex1, graph.VertexWeight(vertex1)); + disjoint_set[vertex1] = vertex1; + + const HashSet<Vertex>& neighbors = graph.Neighbors(vertex1); + for (typename HashSet<Vertex>::const_iterator it2 = neighbors.begin(); + it2 != neighbors.end(); + ++it2) { + const Vertex vertex2 = *it2; + if (vertex1 >= vertex2) { + continue; + } + const double weight = graph.EdgeWeight(vertex1, vertex2); + weighted_edges.push_back( + std::make_pair(weight, std::make_pair(vertex1, vertex2))); + } + } + + // The elements of this vector, are pairs<edge_weight, + // edge>. Sorting it using the reverse iterators gives us the edges + // in decreasing order of edges. + std::sort(weighted_edges.rbegin(), weighted_edges.rend()); + + // Greedily add edges to the spanning tree/forest as long as they do + // not violate the degree/cycle constraint. + for (int i =0; i < weighted_edges.size(); ++i) { + const std::pair<Vertex, Vertex>& edge = weighted_edges[i].second; + const Vertex vertex1 = edge.first; + const Vertex vertex2 = edge.second; + + // Check if either of the vertices are of degree 2 already, in + // which case adding this edge will violate the degree 2 + // constraint. + if ((forest->Neighbors(vertex1).size() == 2) || + (forest->Neighbors(vertex2).size() == 2)) { + continue; + } + + // Find the id of the connected component to which the two + // vertices belong to. If the id is the same, it means that the + // two of them are already connected to each other via some other + // vertex, and adding this edge will create a cycle. + Vertex root1 = FindConnectedComponent(vertex1, &disjoint_set); + Vertex root2 = FindConnectedComponent(vertex2, &disjoint_set); + + if (root1 == root2) { + continue; + } + + // This edge can be added, add an edge in either direction with + // the same weight as the original graph. + const double edge_weight = graph.EdgeWeight(vertex1, vertex2); + forest->AddEdge(vertex1, vertex2, edge_weight); + forest->AddEdge(vertex2, vertex1, edge_weight); + + // Connected the two connected components by updating the + // disjoint_set structure. Always connect the connected component + // with the greater index with the connected component with the + // smaller index. This should ensure shallower trees, for quicker + // lookup. + if (root2 < root1) { + std::swap(root1, root2); + } + + disjoint_set[root2] = root1; + } + return forest; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRAPH_ALGORITHMS_H_ diff --git a/extern/ceres/internal/ceres/householder_vector.h b/extern/ceres/internal/ceres/householder_vector.h new file mode 100644 index 00000000000..f54feea054d --- /dev/null +++ b/extern/ceres/internal/ceres/householder_vector.h @@ -0,0 +1,85 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 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: vitus@google.com (Michael Vitus) + +#ifndef CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_ +#define CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_ + +#include "Eigen/Core" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Algorithm 5.1.1 from 'Matrix Computations' by Golub et al. (Johns Hopkins +// Studies in Mathematical Sciences) but using the nth element of the input +// vector as pivot instead of first. This computes the vector v with v(n) = 1 +// and beta such that H = I - beta * v * v^T is orthogonal and +// H * x = ||x||_2 * e_n. +template <typename Scalar> +void ComputeHouseholderVector(const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& x, + Eigen::Matrix<Scalar, Eigen::Dynamic, 1>* v, + Scalar* beta) { + CHECK_NOTNULL(beta); + CHECK_NOTNULL(v); + CHECK_GT(x.rows(), 1); + CHECK_EQ(x.rows(), v->rows()); + + Scalar sigma = x.head(x.rows() - 1).squaredNorm(); + *v = x; + (*v)(v->rows() - 1) = Scalar(1.0); + + *beta = Scalar(0.0); + const Scalar& x_pivot = x(x.rows() - 1); + + if (sigma <= Scalar(std::numeric_limits<double>::epsilon())) { + if (x_pivot < Scalar(0.0)) { + *beta = Scalar(2.0); + } + return; + } + + const Scalar mu = sqrt(x_pivot * x_pivot + sigma); + Scalar v_pivot = Scalar(1.0); + + if (x_pivot <= Scalar(0.0)) { + v_pivot = x_pivot - mu; + } else { + v_pivot = -sigma / (x_pivot + mu); + } + + *beta = Scalar(2.0) * v_pivot * v_pivot / (sigma + v_pivot * v_pivot); + + v->head(v->rows() - 1) /= v_pivot; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_ diff --git a/extern/ceres/internal/ceres/implicit_schur_complement.cc b/extern/ceres/internal/ceres/implicit_schur_complement.cc new file mode 100644 index 00000000000..d05f03817b7 --- /dev/null +++ b/extern/ceres/internal/ceres/implicit_schur_complement.cc @@ -0,0 +1,225 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/implicit_schur_complement.h" + +#include "Eigen/Dense" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_solver.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +ImplicitSchurComplement::ImplicitSchurComplement( + const LinearSolver::Options& options) + : options_(options), + D_(NULL), + b_(NULL) { +} + +ImplicitSchurComplement::~ImplicitSchurComplement() { +} + +void ImplicitSchurComplement::Init(const BlockSparseMatrix& A, + const double* D, + const double* b) { + // Since initialization is reasonably heavy, perhaps we can save on + // constructing a new object everytime. + if (A_ == NULL) { + A_.reset(PartitionedMatrixViewBase::Create(options_, A)); + } + + D_ = D; + b_ = b; + + // Initialize temporary storage and compute the block diagonals of + // E'E and F'E. + if (block_diagonal_EtE_inverse_ == NULL) { + block_diagonal_EtE_inverse_.reset(A_->CreateBlockDiagonalEtE()); + if (options_.preconditioner_type == JACOBI) { + block_diagonal_FtF_inverse_.reset(A_->CreateBlockDiagonalFtF()); + } + rhs_.resize(A_->num_cols_f()); + rhs_.setZero(); + tmp_rows_.resize(A_->num_rows()); + tmp_e_cols_.resize(A_->num_cols_e()); + tmp_e_cols_2_.resize(A_->num_cols_e()); + tmp_f_cols_.resize(A_->num_cols_f()); + } else { + A_->UpdateBlockDiagonalEtE(block_diagonal_EtE_inverse_.get()); + if (options_.preconditioner_type == JACOBI) { + A_->UpdateBlockDiagonalFtF(block_diagonal_FtF_inverse_.get()); + } + } + + // The block diagonals of the augmented linear system contain + // contributions from the diagonal D if it is non-null. Add that to + // the block diagonals and invert them. + AddDiagonalAndInvert(D_, block_diagonal_EtE_inverse_.get()); + if (options_.preconditioner_type == JACOBI) { + AddDiagonalAndInvert((D_ == NULL) ? NULL : D_ + A_->num_cols_e(), + block_diagonal_FtF_inverse_.get()); + } + + // Compute the RHS of the Schur complement system. + UpdateRhs(); +} + +// Evaluate the product +// +// Sx = [F'F - F'E (E'E)^-1 E'F]x +// +// By breaking it down into individual matrix vector products +// involving the matrices E and F. This is implemented using a +// PartitionedMatrixView of the input matrix A. +void ImplicitSchurComplement::RightMultiply(const double* x, double* y) const { + // y1 = F x + tmp_rows_.setZero(); + A_->RightMultiplyF(x, tmp_rows_.data()); + + // y2 = E' y1 + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); + + // y3 = -(E'E)^-1 y2 + tmp_e_cols_2_.setZero(); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), + tmp_e_cols_2_.data()); + tmp_e_cols_2_ *= -1.0; + + // y1 = y1 + E y3 + A_->RightMultiplyE(tmp_e_cols_2_.data(), tmp_rows_.data()); + + // y5 = D * x + if (D_ != NULL) { + ConstVectorRef Dref(D_ + A_->num_cols_e(), num_cols()); + VectorRef(y, num_cols()) = + (Dref.array().square() * + ConstVectorRef(x, num_cols()).array()).matrix(); + } else { + VectorRef(y, num_cols()).setZero(); + } + + // y = y5 + F' y1 + A_->LeftMultiplyF(tmp_rows_.data(), y); +} + +// Given a block diagonal matrix and an optional array of diagonal +// entries D, add them to the diagonal of the matrix and compute the +// inverse of each diagonal block. +void ImplicitSchurComplement::AddDiagonalAndInvert( + const double* D, + BlockSparseMatrix* block_diagonal) { + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + for (int r = 0; r < block_diagonal_structure->rows.size(); ++r) { + const int row_block_pos = block_diagonal_structure->rows[r].block.position; + const int row_block_size = block_diagonal_structure->rows[r].block.size; + const Cell& cell = block_diagonal_structure->rows[r].cells[0]; + MatrixRef m(block_diagonal->mutable_values() + cell.position, + row_block_size, row_block_size); + + if (D != NULL) { + ConstVectorRef d(D + row_block_pos, row_block_size); + m += d.array().square().matrix().asDiagonal(); + } + + m = m + .selfadjointView<Eigen::Upper>() + .llt() + .solve(Matrix::Identity(row_block_size, row_block_size)); + } +} + +// Similar to RightMultiply, use the block structure of the matrix A +// to compute y = (E'E)^-1 (E'b - E'F x). +void ImplicitSchurComplement::BackSubstitute(const double* x, double* y) { + const int num_cols_e = A_->num_cols_e(); + const int num_cols_f = A_->num_cols_f(); + const int num_cols = A_->num_cols(); + const int num_rows = A_->num_rows(); + + // y1 = F x + tmp_rows_.setZero(); + A_->RightMultiplyF(x, tmp_rows_.data()); + + // y2 = b - y1 + tmp_rows_ = ConstVectorRef(b_, num_rows) - tmp_rows_; + + // y3 = E' y2 + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); + + // y = (E'E)^-1 y3 + VectorRef(y, num_cols).setZero(); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y); + + // The full solution vector y has two blocks. The first block of + // variables corresponds to the eliminated variables, which we just + // computed via back substitution. The second block of variables + // corresponds to the Schur complement system, so we just copy those + // values from the solution to the Schur complement. + VectorRef(y + num_cols_e, num_cols_f) = ConstVectorRef(x, num_cols_f); +} + +// Compute the RHS of the Schur complement system. +// +// rhs = F'b - F'E (E'E)^-1 E'b +// +// Like BackSubstitute, we use the block structure of A to implement +// this using a series of matrix vector products. +void ImplicitSchurComplement::UpdateRhs() { + // y1 = E'b + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(b_, tmp_e_cols_.data()); + + // y2 = (E'E)^-1 y1 + Vector y2 = Vector::Zero(A_->num_cols_e()); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y2.data()); + + // y3 = E y2 + tmp_rows_.setZero(); + A_->RightMultiplyE(y2.data(), tmp_rows_.data()); + + // y3 = b - y3 + tmp_rows_ = ConstVectorRef(b_, A_->num_rows()) - tmp_rows_; + + // rhs = F' y3 + rhs_.setZero(); + A_->LeftMultiplyF(tmp_rows_.data(), rhs_.data()); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/implicit_schur_complement.h b/extern/ceres/internal/ceres/implicit_schur_complement.h new file mode 100644 index 00000000000..5d822ebaeef --- /dev/null +++ b/extern/ceres/internal/ceres/implicit_schur_complement.h @@ -0,0 +1,167 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// An iterative solver for solving the Schur complement/reduced camera +// linear system that arise in SfM problems. + +#ifndef CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ +#define CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ + +#include "ceres/linear_operator.h" +#include "ceres/linear_solver.h" +#include "ceres/partitioned_matrix_view.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; + +// This class implements various linear algebraic operations related +// to the Schur complement without explicitly forming it. +// +// +// Given a reactangular linear system Ax = b, where +// +// A = [E F] +// +// The normal equations are given by +// +// A'Ax = A'b +// +// |E'E E'F||y| = |E'b| +// |F'E F'F||z| |F'b| +// +// and the Schur complement system is given by +// +// [F'F - F'E (E'E)^-1 E'F] z = F'b - F'E (E'E)^-1 E'b +// +// Now if we wish to solve Ax = b in the least squares sense, one way +// is to form this Schur complement system and solve it using +// Preconditioned Conjugate Gradients. +// +// The key operation in a conjugate gradient solver is the evaluation of the +// matrix vector product with the Schur complement +// +// S = F'F - F'E (E'E)^-1 E'F +// +// It is straightforward to see that matrix vector products with S can +// be evaluated without storing S in memory. Instead, given (E'E)^-1 +// (which for our purposes is an easily inverted block diagonal +// matrix), it can be done in terms of matrix vector products with E, +// F and (E'E)^-1. This class implements this functionality and other +// auxilliary bits needed to implement a CG solver on the Schur +// complement using the PartitionedMatrixView object. +// +// THREAD SAFETY: This class is nqot thread safe. In particular, the +// RightMultiply (and the LeftMultiply) methods are not thread safe as +// they depend on mutable arrays used for the temporaries needed to +// compute the product y += Sx; +class ImplicitSchurComplement : public LinearOperator { + public: + // num_eliminate_blocks is the number of E blocks in the matrix + // A. + // + // preconditioner indicates whether the inverse of the matrix F'F + // should be computed or not as a preconditioner for the Schur + // Complement. + // + // TODO(sameeragarwal): Get rid of the two bools below and replace + // them with enums. + explicit ImplicitSchurComplement(const LinearSolver::Options& options); + virtual ~ImplicitSchurComplement(); + + // Initialize the Schur complement for a linear least squares + // problem of the form + // + // |A | x = |b| + // |diag(D)| |0| + // + // If D is null, then it is treated as a zero dimensional matrix. It + // is important that the matrix A have a BlockStructure object + // associated with it and has a block structure that is compatible + // with the SchurComplement solver. + void Init(const 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; + + // The Schur complement is a symmetric positive definite matrix, + // thus the left and right multiply operators are the same. + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + + // y = (E'E)^-1 (E'b - E'F x). Given an estimate of the solution to + // the Schur complement system, this method computes the value of + // the e_block variables that were eliminated to form the Schur + // complement. + void BackSubstitute(const double* x, double* y); + + virtual int num_rows() const { return A_->num_cols_f(); } + virtual int num_cols() const { return A_->num_cols_f(); } + const Vector& rhs() const { return rhs_; } + + const BlockSparseMatrix* block_diagonal_EtE_inverse() const { + return block_diagonal_EtE_inverse_.get(); + } + + const BlockSparseMatrix* block_diagonal_FtF_inverse() const { + return block_diagonal_FtF_inverse_.get(); + } + + private: + void AddDiagonalAndInvert(const double* D, BlockSparseMatrix* matrix); + void UpdateRhs(); + + const LinearSolver::Options& options_; + + scoped_ptr<PartitionedMatrixViewBase> A_; + const double* D_; + const double* b_; + + scoped_ptr<BlockSparseMatrix> block_diagonal_EtE_inverse_; + scoped_ptr<BlockSparseMatrix> block_diagonal_FtF_inverse_; + + Vector rhs_; + + // Temporary storage vectors used to implement RightMultiply. + mutable Vector tmp_rows_; + mutable Vector tmp_e_cols_; + mutable Vector tmp_e_cols_2_; + mutable Vector tmp_f_cols_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ diff --git a/extern/ceres/internal/ceres/integral_types.h b/extern/ceres/internal/ceres/integral_types.h new file mode 100644 index 00000000000..98a746f13ff --- /dev/null +++ b/extern/ceres/internal/ceres/integral_types.h @@ -0,0 +1,91 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Portable typedefs for various fixed-size integers. Uses template +// metaprogramming instead of fragile compiler defines. + +#ifndef CERES_INTERNAL_INTEGRAL_TYPES_H_ +#define CERES_INTERNAL_INTEGRAL_TYPES_H_ + +namespace ceres { +namespace internal { + +// Compile time ternary on types. +template<bool kCondition, typename kTrueType, typename kFalseType> +struct Ternary { + typedef kTrueType type; +}; +template<typename kTrueType, typename kFalseType> +struct Ternary<false, kTrueType, kFalseType> { + typedef kFalseType type; +}; + +#define CERES_INTSIZE(TYPE) \ + typename Ternary<sizeof(TYPE) * 8 == kBits, TYPE, + +template<int kBits> +struct Integer { + typedef + CERES_INTSIZE(char) + CERES_INTSIZE(short) + CERES_INTSIZE(int) + CERES_INTSIZE(long int) + CERES_INTSIZE(long long) + void>::type >::type >::type >::type >::type + type; +}; + +template<int kBits> +struct UnsignedInteger { + typedef + CERES_INTSIZE(unsigned char) + CERES_INTSIZE(unsigned short) + CERES_INTSIZE(unsigned int) + CERES_INTSIZE(unsigned long int) + CERES_INTSIZE(unsigned long long) + void>::type >::type >::type >::type >::type + type; +}; + +#undef CERES_INTSIZE + +typedef Integer< 8>::type int8; +typedef Integer<32>::type int32; +typedef Integer<64>::type int64; + +typedef UnsignedInteger< 8>::type uint8; +typedef UnsignedInteger<16>::type uint16; +typedef UnsignedInteger<32>::type uint32; +typedef UnsignedInteger<64>::type uint64; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_INTEGRAL_TYPES_H_ diff --git a/extern/ceres/internal/ceres/iterative_schur_complement_solver.cc b/extern/ceres/internal/ceres/iterative_schur_complement_solver.cc new file mode 100644 index 00000000000..9d4e30d69d2 --- /dev/null +++ b/extern/ceres/internal/ceres/iterative_schur_complement_solver.cc @@ -0,0 +1,182 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/iterative_schur_complement_solver.h" + +#include <algorithm> +#include <cstring> +#include <vector> + +#include "Eigen/Dense" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/conjugate_gradients_solver.h" +#include "ceres/detect_structure.h" +#include "ceres/implicit_schur_complement.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_solver.h" +#include "ceres/preconditioner.h" +#include "ceres/schur_jacobi_preconditioner.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "ceres/visibility_based_preconditioner.h" +#include "ceres/wall_time.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +IterativeSchurComplementSolver::IterativeSchurComplementSolver( + const LinearSolver::Options& options) + : options_(options) { +} + +IterativeSchurComplementSolver::~IterativeSchurComplementSolver() { +} + +LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( + BlockSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + EventLogger event_logger("IterativeSchurComplementSolver::Solve"); + + CHECK_NOTNULL(A->block_structure()); + const int num_eliminate_blocks = options_.elimination_groups[0]; + // Initialize a ImplicitSchurComplement object. + if (schur_complement_ == NULL) { + DetectStructure(*(A->block_structure()), + num_eliminate_blocks, + &options_.row_block_size, + &options_.e_block_size, + &options_.f_block_size); + schur_complement_.reset(new ImplicitSchurComplement(options_)); + } + schur_complement_->Init(*A, per_solve_options.D, b); + + const int num_schur_complement_blocks = + A->block_structure()->cols.size() - num_eliminate_blocks; + 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 = LINEAR_SOLVER_SUCCESS; + schur_complement_->BackSubstitute(NULL, x); + return cg_summary; + } + + // Initialize the solution to the Schur complement system to zero. + reduced_linear_system_solution_.resize(schur_complement_->num_rows()); + reduced_linear_system_solution_.setZero(); + + // Instantiate a conjugate gradient solver that runs on the Schur + // complement matrix with the block diagonal of the matrix F'F as + // the preconditioner. + LinearSolver::Options cg_options; + cg_options.min_num_iterations = options_.min_num_iterations; + cg_options.max_num_iterations = options_.max_num_iterations; + ConjugateGradientsSolver cg_solver(cg_options); + LinearSolver::PerSolveOptions cg_per_solve_options; + + cg_per_solve_options.r_tolerance = per_solve_options.r_tolerance; + cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance; + + Preconditioner::Options preconditioner_options; + preconditioner_options.type = options_.preconditioner_type; + preconditioner_options.visibility_clustering_type = + options_.visibility_clustering_type; + 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; + preconditioner_options.f_block_size = options_.f_block_size; + preconditioner_options.elimination_groups = options_.elimination_groups; + + switch (options_.preconditioner_type) { + case IDENTITY: + break; + case JACOBI: + preconditioner_.reset( + new SparseMatrixPreconditionerWrapper( + schur_complement_->block_diagonal_FtF_inverse())); + break; + case SCHUR_JACOBI: + if (preconditioner_.get() == NULL) { + preconditioner_.reset( + 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)); + } + break; + default: + LOG(FATAL) << "Unknown Preconditioner Type"; + } + + bool preconditioner_update_was_successful = true; + if (preconditioner_.get() != NULL) { + preconditioner_update_was_successful = + preconditioner_->Update(*A, per_solve_options.D); + cg_per_solve_options.preconditioner = preconditioner_.get(); + } + event_logger.AddEvent("Setup"); + + LinearSolver::Summary cg_summary; + cg_summary.num_iterations = 0; + cg_summary.termination_type = LINEAR_SOLVER_FAILURE; + + // TODO(sameeragarwal): Refactor preconditioners to return a more + // sane message. + cg_summary.message = "Preconditioner update failed."; + if (preconditioner_update_was_successful) { + cg_summary = cg_solver.Solve(schur_complement_.get(), + schur_complement_->rhs().data(), + cg_per_solve_options, + reduced_linear_system_solution_.data()); + if (cg_summary.termination_type != LINEAR_SOLVER_FAILURE && + cg_summary.termination_type != LINEAR_SOLVER_FATAL_ERROR) { + schur_complement_->BackSubstitute( + reduced_linear_system_solution_.data(), x); + } + } + event_logger.AddEvent("Solve"); + return cg_summary; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/iterative_schur_complement_solver.h b/extern/ceres/internal/ceres/iterative_schur_complement_solver.h new file mode 100644 index 00000000000..e90d310de07 --- /dev/null +++ b/extern/ceres/internal/ceres/iterative_schur_complement_solver.h @@ -0,0 +1,92 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ +#define CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; +class ImplicitSchurComplement; +class Preconditioner; + +// This class implements an iterative solver for the linear least +// squares problems that have a bi-partite sparsity structure common +// to Structure from Motion problems. +// +// The algorithm used by this solver was developed in a series of +// papers - "Agarwal et al, Bundle Adjustment in the Large, ECCV 2010" +// and "Wu et al, Multicore Bundle Adjustment, submitted to CVPR +// 2011" at the Univeristy of Washington. +// +// The key idea is that one can run Conjugate Gradients on the Schur +// Complement system without explicitly forming the Schur Complement +// in memory. The heavy lifting for this is done by the +// ImplicitSchurComplement class. Not forming the Schur complement in +// memory and factoring it results in substantial savings in time and +// memory. Further, iterative solvers like this open up the +// possibility of solving the Newton equations in a non-linear solver +// only approximately and terminating early, thereby saving even more +// time. +// +// For the curious, running CG on the Schur complement is the same as +// running CG on the Normal Equations with an SSOR preconditioner. For +// a proof of this fact and others related to this solver please see +// the section on Domain Decomposition Methods in Saad's book +// "Iterative Methods for Sparse Linear Systems". +class IterativeSchurComplementSolver : public BlockSparseMatrixSolver { + public: + explicit IterativeSchurComplementSolver(const LinearSolver::Options& options); + virtual ~IterativeSchurComplementSolver(); + + private: + virtual LinearSolver::Summary SolveImpl( + BlockSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& options, + double* x); + + LinearSolver::Options options_; + scoped_ptr<internal::ImplicitSchurComplement> schur_complement_; + scoped_ptr<Preconditioner> preconditioner_; + Vector reduced_linear_system_solution_; + CERES_DISALLOW_COPY_AND_ASSIGN(IterativeSchurComplementSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/lapack.cc b/extern/ceres/internal/ceres/lapack.cc new file mode 100644 index 00000000000..6fc23f4e658 --- /dev/null +++ b/extern/ceres/internal/ceres/lapack.cc @@ -0,0 +1,193 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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 "ceres/internal/port.h" +#include "ceres/linear_solver.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 { + +LinearSolverTerminationType LAPACK::SolveInPlaceUsingCholesky( + int num_rows, + const double* in_lhs, + double* rhs_and_solution, + std::string* message) { +#ifdef CERES_NO_LAPACK + LOG(FATAL) << "Ceres was built without a BLAS library."; + return LINEAR_SOLVER_FATAL_ERROR; +#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(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it." + << "LAPACK::dpotrf fatal error." + << "Argument: " << -info << " is invalid."; + return LINEAR_SOLVER_FATAL_ERROR; + } + + if (info > 0) { + *message = + StringPrintf( + "LAPACK::dpotrf numerical failure. " + "The leading minor of order %d is not positive definite.", info); + return LINEAR_SOLVER_FAILURE; + } + + dpotrs_(&uplo, &n, &nrhs, lhs, &n, rhs_and_solution, &n, &info); + if (info < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it." + << "LAPACK::dpotrs fatal error." + << "Argument: " << -info << " is invalid."; + return LINEAR_SOLVER_FATAL_ERROR; + } + + *message = "Success"; + return LINEAR_SOLVER_SUCCESS; +#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); + + if (info < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it." + << "LAPACK::dgels fatal error." + << "Argument: " << -info << " is invalid."; + } + return static_cast<int>(work); +#endif +} + +LinearSolverTerminationType LAPACK::SolveInPlaceUsingQR( + int num_rows, + int num_cols, + const double* in_lhs, + int work_size, + double* work, + double* rhs_and_solution, + std::string* message) { +#ifdef CERES_NO_LAPACK + LOG(FATAL) << "Ceres was built without a LAPACK library."; + return LINEAR_SOLVER_FATAL_ERROR; +#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); + + if (info < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it." + << "LAPACK::dgels fatal error." + << "Argument: " << -info << " is invalid."; + } + + *message = "Success."; + return LINEAR_SOLVER_SUCCESS; +#endif +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/lapack.h b/extern/ceres/internal/ceres/lapack.h new file mode 100644 index 00000000000..5bb1a220c26 --- /dev/null +++ b/extern/ceres/internal/ceres/lapack.h @@ -0,0 +1,100 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_ + +#include <string> +#include "ceres/internal/port.h" +#include "ceres/linear_solver.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 and the message string together describe whether + // the solver terminated successfully or not and if so, what was the + // reason for failure. + static LinearSolverTerminationType SolveInPlaceUsingCholesky( + int num_rows, + const double* lhs, + double* rhs_and_solution, + std::string* message); + + // 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 and the message string together describe whether + // the solver terminated successfully or not and if so, what was the + // reason for failure. + static LinearSolverTerminationType SolveInPlaceUsingQR( + int num_rows, + int num_cols, + const double* lhs, + int work_size, + double* work, + double* rhs_and_solution, + std::string* message); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LAPACK_H_ diff --git a/extern/ceres/internal/ceres/levenberg_marquardt_strategy.cc b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.cc new file mode 100644 index 00000000000..e9833805ef5 --- /dev/null +++ b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.cc @@ -0,0 +1,167 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/levenberg_marquardt_strategy.h" + +#include <cmath> +#include "Eigen/Core" +#include "ceres/array_utils.h" +#include "ceres/internal/eigen.h" +#include "ceres/linear_least_squares_problems.h" +#include "ceres/linear_solver.h" +#include "ceres/sparse_matrix.h" +#include "ceres/trust_region_strategy.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +LevenbergMarquardtStrategy::LevenbergMarquardtStrategy( + const TrustRegionStrategy::Options& options) + : linear_solver_(options.linear_solver), + radius_(options.initial_radius), + max_radius_(options.max_radius), + min_diagonal_(options.min_lm_diagonal), + max_diagonal_(options.max_lm_diagonal), + decrease_factor_(2.0), + reuse_diagonal_(false) { + CHECK_NOTNULL(linear_solver_); + CHECK_GT(min_diagonal_, 0.0); + CHECK_LE(min_diagonal_, max_diagonal_); + CHECK_GT(max_radius_, 0.0); +} + +LevenbergMarquardtStrategy::~LevenbergMarquardtStrategy() { +} + +TrustRegionStrategy::Summary LevenbergMarquardtStrategy::ComputeStep( + const TrustRegionStrategy::PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals, + double* step) { + CHECK_NOTNULL(jacobian); + CHECK_NOTNULL(residuals); + CHECK_NOTNULL(step); + + const int num_parameters = jacobian->num_cols(); + if (!reuse_diagonal_) { + if (diagonal_.rows() != num_parameters) { + diagonal_.resize(num_parameters, 1); + } + + jacobian->SquaredColumnNorm(diagonal_.data()); + for (int i = 0; i < num_parameters; ++i) { + diagonal_[i] = std::min(std::max(diagonal_[i], min_diagonal_), + max_diagonal_); + } + } + + lm_diagonal_ = (diagonal_ / radius_).array().sqrt(); + + LinearSolver::PerSolveOptions solve_options; + solve_options.D = lm_diagonal_.data(); + solve_options.q_tolerance = per_solve_options.eta; + // Disable r_tolerance checking. Since we only care about + // termination via the q_tolerance. As Nash and Sofer show, + // r_tolerance based termination is essentially useless in + // Truncated Newton methods. + solve_options.r_tolerance = -1.0; + + // Invalidate the output array lm_step, so that we can detect if + // the linear solver generated numerical garbage. This is known + // to happen for the DENSE_QR and then DENSE_SCHUR solver when + // the Jacobin is severly rank deficient and mu is too small. + InvalidateArray(num_parameters, step); + + // Instead of solving Jx = -r, solve Jy = r. + // Then x can be found as x = -y, but the inputs jacobian and residuals + // do not need to be modified. + LinearSolver::Summary linear_solver_summary = + linear_solver_->Solve(jacobian, residuals, solve_options, step); + + if (linear_solver_summary.termination_type == LINEAR_SOLVER_FATAL_ERROR) { + LOG(WARNING) << "Linear solver fatal error: " + << linear_solver_summary.message; + } else if (linear_solver_summary.termination_type == LINEAR_SOLVER_FAILURE) { + LOG(WARNING) << "Linear solver failure. Failed to compute a step: " + << linear_solver_summary.message; + } else if (!IsArrayValid(num_parameters, step)) { + LOG(WARNING) << "Linear solver failure. Failed to compute a finite step."; + linear_solver_summary.termination_type = LINEAR_SOLVER_FAILURE; + } 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; + summary.termination_type = linear_solver_summary.termination_type; + return summary; +} + +void LevenbergMarquardtStrategy::StepAccepted(double step_quality) { + CHECK_GT(step_quality, 0.0); + radius_ = radius_ / std::max(1.0 / 3.0, + 1.0 - pow(2.0 * step_quality - 1.0, 3)); + radius_ = std::min(max_radius_, radius_); + decrease_factor_ = 2.0; + reuse_diagonal_ = false; +} + +void LevenbergMarquardtStrategy::StepRejected(double step_quality) { + radius_ = radius_ / decrease_factor_; + decrease_factor_ *= 2.0; + reuse_diagonal_ = true; +} + +double LevenbergMarquardtStrategy::Radius() const { + return radius_; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/levenberg_marquardt_strategy.h b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.h new file mode 100644 index 00000000000..c87a016c8f4 --- /dev/null +++ b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.h @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_LEVENBERG_MARQUARDT_STRATEGY_H_ +#define CERES_INTERNAL_LEVENBERG_MARQUARDT_STRATEGY_H_ + +#include "ceres/internal/eigen.h" +#include "ceres/trust_region_strategy.h" + +namespace ceres { +namespace internal { + +// Levenberg-Marquardt step computation and trust region sizing +// strategy based on on "Methods for Nonlinear Least Squares" by +// K. Madsen, H.B. Nielsen and O. Tingleff. Available to download from +// +// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf +class LevenbergMarquardtStrategy : public TrustRegionStrategy { + public: + explicit LevenbergMarquardtStrategy( + const TrustRegionStrategy::Options& options); + virtual ~LevenbergMarquardtStrategy(); + + // TrustRegionStrategy interface + virtual TrustRegionStrategy::Summary ComputeStep( + const TrustRegionStrategy::PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals, + double* step); + virtual void StepAccepted(double step_quality); + virtual void StepRejected(double step_quality); + virtual void StepIsInvalid() { + // Treat the current step as a rejected step with no increase in + // solution quality. Since rejected steps lead to decrease in the + // size of the trust region, the next time ComputeStep is called, + // this will lead to a better conditioned system. + StepRejected(0.0); + } + + virtual double Radius() const; + + private: + LinearSolver* linear_solver_; + double radius_; + double max_radius_; + const double min_diagonal_; + const double max_diagonal_; + double decrease_factor_; + bool reuse_diagonal_; + Vector diagonal_; // diagonal_ = diag(J'J) + // Scaled copy of diagonal_. Stored here as optimization to prevent + // allocations in every iteration and reuse when a step fails and + // ComputeStep is called again. + Vector lm_diagonal_; // lm_diagonal_ = diagonal_ / radius_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LEVENBERG_MARQUARDT_STRATEGY_H_ diff --git a/extern/ceres/internal/ceres/line_search.cc b/extern/ceres/internal/ceres/line_search.cc new file mode 100644 index 00000000000..9cdcb7b77e5 --- /dev/null +++ b/extern/ceres/internal/ceres/line_search.cc @@ -0,0 +1,881 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/line_search.h" + +#include <iomanip> +#include <iostream> // NOLINT + +#include "glog/logging.h" +#include "ceres/evaluator.h" +#include "ceres/internal/eigen.h" +#include "ceres/fpclassify.h" +#include "ceres/map_util.h" +#include "ceres/polynomial.h" +#include "ceres/stringprintf.h" +#include "ceres/wall_time.h" + +namespace ceres { +namespace internal { + +using std::map; +using std::ostream; +using std::string; +using std::vector; + +namespace { +// Precision used for floating point values in error message output. +const int kErrorMessageNumericPrecision = 8; + +FunctionSample ValueSample(const double x, const double value) { + FunctionSample sample; + sample.x = x; + sample.value = value; + sample.value_is_valid = true; + return sample; +} + +FunctionSample ValueAndGradientSample(const double x, + const double value, + const double gradient) { + FunctionSample sample; + sample.x = x; + sample.value = value; + sample.gradient = gradient; + sample.value_is_valid = true; + sample.gradient_is_valid = true; + return sample; +} + +} // namespace + + +ostream& operator<<(ostream &os, const FunctionSample& sample); + +// Convenience stream operator for pushing FunctionSamples into log messages. +ostream& operator<<(ostream &os, const FunctionSample& sample) { + os << sample.ToDebugString(); + 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()), + direction_(evaluator->NumEffectiveParameters()), + evaluation_point_(evaluator->NumParameters()), + scaled_direction_(evaluator->NumEffectiveParameters()), + gradient_(evaluator->NumEffectiveParameters()), + initial_evaluator_residual_time_in_seconds(0.0), + initial_evaluator_jacobian_time_in_seconds(0.0) {} + +void LineSearchFunction::Init(const Vector& position, + const Vector& direction) { + position_ = position; + direction_ = direction; +} + +bool LineSearchFunction::Evaluate(double x, double* f, double* g) { + scaled_direction_ = x * direction_; + if (!evaluator_->Plus(position_.data(), + scaled_direction_.data(), + evaluation_point_.data())) { + return false; + } + + if (g == NULL) { + return (evaluator_->Evaluate(evaluation_point_.data(), + f, NULL, NULL, NULL) && + IsFinite(*f)); + } + + if (!evaluator_->Evaluate(evaluation_point_.data(), + f, NULL, gradient_.data(), NULL)) { + return false; + } + + *g = direction_.dot(gradient_); + return IsFinite(*f) && IsFinite(*g); +} + +double LineSearchFunction::DirectionInfinityNorm() const { + return direction_.lpNorm<Eigen::Infinity>(); +} + +void LineSearchFunction::ResetTimeStatistics() { + const map<string, double> evaluator_time_statistics = + evaluator_->TimeStatistics(); + initial_evaluator_residual_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0); + initial_evaluator_jacobian_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0); +} + +void LineSearchFunction::TimeStatistics( + double* cost_evaluation_time_in_seconds, + double* gradient_evaluation_time_in_seconds) const { + const map<string, double> evaluator_time_statistics = + evaluator_->TimeStatistics(); + *cost_evaluation_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0) - + initial_evaluator_residual_time_in_seconds; + // Strictly speaking this will slightly underestimate the time spent + // evaluating the gradient of the line search univariate cost function as it + // does not count the time spent performing the dot product with the direction + // vector. However, this will typically be small by comparison, and also + // allows direct subtraction of the timing information from the totals for + // the evaluator returned in the solver summary. + *gradient_evaluation_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0) - + initial_evaluator_jacobian_time_in_seconds; +} + +void LineSearch::Search(double step_size_estimate, + double initial_cost, + double initial_gradient, + Summary* summary) const { + const double start_time = WallTimeInSeconds(); + *CHECK_NOTNULL(summary) = LineSearch::Summary(); + + summary->cost_evaluation_time_in_seconds = 0.0; + summary->gradient_evaluation_time_in_seconds = 0.0; + summary->polynomial_minimization_time_in_seconds = 0.0; + + options().function->ResetTimeStatistics(); + this->DoSearch(step_size_estimate, initial_cost, initial_gradient, summary); + options().function-> + TimeStatistics(&summary->cost_evaluation_time_in_seconds, + &summary->gradient_evaluation_time_in_seconds); + + summary->total_time_in_seconds = WallTimeInSeconds() - start_time; +} + +// 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 std::min(std::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) + << std::scientific << std::setprecision(kErrorMessageNumericPrecision) + << "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::DoSearch(const double step_size_estimate, + const double initial_cost, + const double initial_gradient, + Summary* summary) const { + 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); + LineSearchFunction* 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; + + // As the Armijo line search algorithm always uses the initial point, for + // which both the function value and derivative are known, when fitting a + // minimizing polynomial, we can fit up to a quadratic without requiring the + // gradient at the current query point. + const bool interpolation_uses_gradient_at_current_sample = + options().interpolation_type == CUBIC; + const double descent_direction_max_norm = function->DirectionInfinityNorm(); + + ++summary->num_function_evaluations; + if (interpolation_uses_gradient_at_current_sample) { + ++summary->num_gradient_evaluations; + } + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + interpolation_uses_gradient_at_current_sample + ? ¤t.gradient : NULL); + current.gradient_is_valid = + interpolation_uses_gradient_at_current_sample && 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_IF(WARNING, !options().is_silent) << summary->error; + return; + } + + const double polynomial_minimization_start_time = WallTimeInSeconds(); + 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)); + summary->polynomial_minimization_time_in_seconds += + (WallTimeInSeconds() - polynomial_minimization_start_time); + + 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_IF(WARNING, !options().is_silent) << summary->error; + return; + } + + previous = current; + current.x = step_size; + + ++summary->num_function_evaluations; + if (interpolation_uses_gradient_at_current_sample) { + ++summary->num_gradient_evaluations; + } + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + interpolation_uses_gradient_at_current_sample + ? ¤t.gradient : NULL); + current.gradient_is_valid = + interpolation_uses_gradient_at_current_sample && current.value_is_valid; + } + + summary->optimal_step_size = current.x; + summary->success = true; +} + +WolfeLineSearch::WolfeLineSearch(const LineSearch::Options& options) + : LineSearch(options) {} + +void WolfeLineSearch::DoSearch(const double step_size_estimate, + const double initial_cost, + const double initial_gradient, + Summary* summary) const { + // 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)) { + // Failed to find either a valid point, a valid bracket satisfying the Wolfe + // conditions, or even a step size > minimum tolerance satisfying the Armijo + // condition. + 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, or whilst searching + // shrank the bracket width until it was below our minimum tolerance. + // As these are 'artificial' constraints, and we would otherwise fail to + // produce a valid point when ArmijoLineSearch would succeed, we return the + // point with the lowest cost found thus far which satsifies the Armijo + // condition (but not the Wolfe conditions). + summary->optimal_step_size = bracket_low.x; + summary->success = true; + return; + } + + VLOG(3) << std::scientific << std::setprecision(kErrorMessageNumericPrecision) + << "Starting line search zoom phase with bracket_low: " + << bracket_low << ", bracket_high: " << bracket_high + << ", bracket width: " << fabs(bracket_low.x - bracket_high.x) + << ", bracket abs delta cost: " + << fabs(bracket_low.value - bracket_high.value); + + // 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 = solution.x; + summary->success = true; +} + +// Returns true if either: +// +// A termination condition satisfying the (strong) Wolfe bracketing conditions +// is found: +// +// - A valid point, defined as a bracket of zero width [zoom not required]. +// - A valid bracket (of width > tolerance), [zoom required]. +// +// Or, searching was stopped due to an 'artificial' constraint, i.e. not +// a condition imposed / required by the underlying algorithm, but instead an +// engineering / implementation consideration. But a step which exceeds the +// minimum step size, and satsifies the Armijo condition was still found, +// and should thus be used [zoom not required]. +// +// Returns false if no step size > minimum step size was found which +// satisfies at least the Armijo condition. +bool WolfeLineSearch::BracketingPhase( + const FunctionSample& initial_position, + const double step_size_estimate, + FunctionSample* bracket_low, + FunctionSample* bracket_high, + bool* do_zoom_search, + Summary* summary) const { + LineSearchFunction* function = options().function; + + FunctionSample previous = initial_position; + FunctionSample current = ValueAndGradientSample(step_size_estimate, 0.0, 0.0); + current.value_is_valid = false; + + const double descent_direction_max_norm = + function->DirectionInfinityNorm(); + + *do_zoom_search = false; + *bracket_low = initial_position; + + // As we require the gradient to evaluate the Wolfe condition, we always + // calculate it together with the value, irrespective of the interpolation + // type. As opposed to only calculating the gradient after the Armijo + // condition is satisifed, as the computational saving from this approach + // would be slight (perhaps even negative due to the extra call). Also, + // always calculating the value & gradient together protects against us + // reporting invalid solutions if the cost function returns slightly different + // function values when evaluated with / without gradients (due to numerical + // issues). + ++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; + + 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; + VLOG(3) << std::scientific + << std::setprecision(kErrorMessageNumericPrecision) + << "Bracket found: current step (" << current.x + << ") violates Armijo sufficient condition, or has passed an " + << "inflection point of f() based on value."; + break; + } + + 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; + VLOG(3) << std::scientific + << std::setprecision(kErrorMessageNumericPrecision) + << "Bracketing phase found step size: " << current.x + << ", satisfying strong Wolfe conditions, initial_position: " + << initial_position << ", current: " << 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; + VLOG(3) << "Bracket found: current step (" << current.x + << ") satisfies Armijo, but has gradient >= 0, thus have passed " + << "an inflection point of f()."; + break; + + } else if (current.value_is_valid && + fabs(current.x - previous.x) * descent_direction_max_norm + < options().min_step_size) { + // We have shrunk the search bracket to a width less than our tolerance, + // and still not found either a point satisfying the strong Wolfe + // conditions, or a valid bracket containing such a point. Stop searching + // and set bracket_low to the size size amongst all those tested which + // minimizes f() and satisfies the Armijo condition. + LOG_IF(WARNING, !options().is_silent) + << "Line search failed: Wolfe bracketing phase shrank " + << "bracket width: " << fabs(current.x - previous.x) + << ", to < tolerance: " << options().min_step_size + << ", with descent_direction_max_norm: " + << descent_direction_max_norm << ", and failed to find " + << "a point satisfying the strong Wolfe conditions or a " + << "bracketing containing such a point. Accepting " + << "point found satisfying Armijo condition only, to " + << "allow continuation."; + *bracket_low = current; + 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_IF(WARNING, !options().is_silent) << 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; + break; + } + // 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 polynomial_minimization_start_time = WallTimeInSeconds(); + const double step_size = + this->InterpolatingPolynomialMinimizingStepSize( + options().interpolation_type, + previous, + unused_previous, + current, + previous.x, + max_step_size); + summary->polynomial_minimization_time_in_seconds += + (WallTimeInSeconds() - polynomial_minimization_start_time); + 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_IF(WARNING, !options().is_silent) << summary->error; + return false; + } + + previous = current.value_is_valid ? current : previous; + current.x = step_size; + + ++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; + } + + // Ensure that even if a valid bracket was found, we will only mark a zoom + // as required if the bracket's width is greater than our minimum tolerance. + if (*do_zoom_search && + fabs(bracket_high->x - bracket_low->x) * descent_direction_max_norm + < options().min_step_size) { + *do_zoom_search = false; + } + + 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) const { + LineSearchFunction* function = options().function; + + CHECK(bracket_low.value_is_valid && bracket_low.gradient_is_valid) + << std::scientific << std::setprecision(kErrorMessageNumericPrecision) + << "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). + // + // We also do not require that: bracket_low.value < bracket_high.value, + // although this is typical. This is to deal with the case when + // bracket_low = initial_position, bracket_high is the first sample, + // and bracket_high does not satisfy the Armijo condition, but still has + // bracket_high.value < initial_position.value. + CHECK(bracket_high.value_is_valid) + << std::scientific << std::setprecision(kErrorMessageNumericPrecision) + << "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; + + if (bracket_low.gradient * (bracket_high.x - bracket_low.x) >= 0) { + // The third condition for a valid initial bracket: + // + // 3. bracket_high is chosen after bracket_low, s.t. + // bracket_low.gradient * (bracket_high.x - bracket_low.x) < 0. + // + // is not satisfied. As this can happen when the users' cost function + // returns inconsistent gradient values relative to the function values, + // we do not CHECK_LT(), but we do stop processing and return an invalid + // value. + summary->error = + StringPrintf("Line search failed: Wolfe zoom phase passed a bracket " + "which does not satisfy: bracket_low.gradient * " + "(bracket_high.x - bracket_low.x) < 0 [%.8e !< 0] " + "with initial_position: %s, bracket_low: %s, bracket_high:" + " %s, the most likely cause of which is the cost function " + "returning inconsistent gradient & function values.", + bracket_low.gradient * (bracket_high.x - bracket_low.x), + initial_position.ToDebugString().c_str(), + bracket_low.ToDebugString().c_str(), + bracket_high.ToDebugString().c_str()); + LOG_IF(WARNING, !options().is_silent) << summary->error; + solution->value_is_valid = false; + return false; + } + + const int num_bracketing_iterations = summary->num_iterations; + const double descent_direction_max_norm = 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_IF(WARNING, !options().is_silent) << 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_IF(WARNING, !options().is_silent) << 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); + const double polynomial_minimization_start_time = WallTimeInSeconds(); + solution->x = + this->InterpolatingPolynomialMinimizingStepSize( + options().interpolation_type, + lower_bound_step, + unused_previous, + upper_bound_step, + lower_bound_step.x, + upper_bound_step.x); + summary->polynomial_minimization_time_in_seconds += + (WallTimeInSeconds() - polynomial_minimization_start_time); + // 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. + // + // As we require the gradient to evaluate the Wolfe condition, we always + // calculate it together with the value, irrespective of the interpolation + // type. As opposed to only calculating the gradient after the Armijo + // condition is satisifed, as the computational saving from this approach + // would be slight (perhaps even negative due to the extra call). Also, + // always calculating the value & gradient together protects against us + // reporting invalid solutions if the cost function returns slightly + // different function values when evaluated with / without gradients (due + // to numerical issues). + ++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_IF(WARNING, !options().is_silent) << summary->error; + return false; + } + + VLOG(3) << "Zoom iteration: " + << summary->num_iterations - num_bracketing_iterations + << ", bracket_low: " << bracket_low + << ", bracket_high: " << bracket_high + << ", minimizing solution: " << *solution; + + 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 (fabs(solution->gradient) <= + -options().sufficient_curvature_decrease * initial_position.gradient) { + // Found a valid termination point satisfying strong Wolfe conditions. + VLOG(3) << std::scientific + << std::setprecision(kErrorMessageNumericPrecision) + << "Zoom phase found step size: " << solution->x + << ", 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/ceres/internal/ceres/line_search.h b/extern/ceres/internal/ceres/line_search.h new file mode 100644 index 00000000000..6a21cbeac11 --- /dev/null +++ b/extern/ceres/internal/ceres/line_search.h @@ -0,0 +1,327 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Interface for and implementation of various Line search algorithms. + +#ifndef CERES_INTERNAL_LINE_SEARCH_H_ +#define CERES_INTERNAL_LINE_SEARCH_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; +class LineSearchFunction; + +// Line search is another name for a one dimensional optimization +// algorithm. The name "line search" comes from the fact one +// dimensional optimization problems that arise as subproblems of +// general multidimensional optimization problems. +// +// While finding the exact minimum of a one dimensionl function is +// hard, instances of LineSearch find a point that satisfies a +// sufficient decrease condition. Depending on the particular +// condition used, we get a variety of different line search +// algorithms, e.g., Armijo, Wolfe etc. +class LineSearch { + public: + struct Summary; + + struct Options { + Options() + : interpolation_type(CUBIC), + sufficient_decrease(1e-4), + 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), + is_silent(false), + function(NULL) {} + + // Degree of the polynomial used to approximate the objective + // function. + LineSearchInterpolationType interpolation_type; + + // Armijo and Wolfe 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 sufficient_decrease; + + // 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 + // + double max_step_contraction; + + // In each iteration of the Armijo / Wolfe line search, + // + // 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 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; + + bool is_silent; + + // The one dimensional function that the line search algorithm + // minimizes. + LineSearchFunction* function; + }; + + // Result of the line search. + struct Summary { + Summary() + : success(false), + optimal_step_size(0.0), + num_function_evaluations(0), + num_gradient_evaluations(0), + num_iterations(0), + cost_evaluation_time_in_seconds(-1.0), + gradient_evaluation_time_in_seconds(-1.0), + polynomial_minimization_time_in_seconds(-1.0), + total_time_in_seconds(-1.0) {} + + bool success; + double optimal_step_size; + int num_function_evaluations; + int num_gradient_evaluations; + int num_iterations; + // Cumulative time spent evaluating the value of the cost function across + // all iterations. + double cost_evaluation_time_in_seconds; + // Cumulative time spent evaluating the gradient of the cost function across + // all iterations. + double gradient_evaluation_time_in_seconds; + // Cumulative time spent minimizing the interpolating polynomial to compute + // the next candidate step size across all iterations. + double polynomial_minimization_time_in_seconds; + double total_time_in_seconds; + std::string error; + }; + + explicit LineSearch(const LineSearch::Options& options); + virtual ~LineSearch() {} + + static LineSearch* Create(const LineSearchType line_search_type, + const LineSearch::Options& options, + std::string* error); + + // Perform the line search. + // + // step_size_estimate must be a positive number. + // + // initial_cost and initial_gradient are the values and gradient of + // the function at zero. + // summary must not be null and will contain the result of the line + // search. + // + // Summary::success is true if a non-zero step size is found. + void Search(double step_size_estimate, + double initial_cost, + double initial_gradient, + Summary* summary) const; + 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: + virtual void DoSearch(double step_size_estimate, + double initial_cost, + double initial_gradient, + Summary* summary) const = 0; + + private: + LineSearch::Options options_; +}; + +// An object used by the line search to access the function values +// and gradient of the one dimensional function being optimized. +// +// In practice, this object provides access to the objective +// function value and the directional derivative of the underlying +// optimization problem along a specific search direction. +class LineSearchFunction { + public: + explicit LineSearchFunction(Evaluator* evaluator); + void Init(const Vector& position, const Vector& direction); + // Evaluate the line search objective + // + // f(x) = p(position + x * direction) + // + // Where, p is the objective function of the general optimization + // problem. + // + // g is the gradient f'(x) at x. + // + // f must not be null. The gradient is computed only if g is not null. + bool Evaluate(double x, double* f, double* g); + double DirectionInfinityNorm() const; + // Resets to now, the start point for the results from TimeStatistics(). + void ResetTimeStatistics(); + void TimeStatistics(double* cost_evaluation_time_in_seconds, + double* gradient_evaluation_time_in_seconds) const; + + private: + Evaluator* evaluator_; + Vector position_; + Vector direction_; + + // evaluation_point = Evaluator::Plus(position_, x * direction_); + Vector evaluation_point_; + + // scaled_direction = x * direction_; + Vector scaled_direction_; + Vector gradient_; + + // We may not exclusively own the evaluator (e.g. in the Trust Region + // minimizer), hence we need to save the initial evaluation durations for the + // value & gradient to accurately determine the duration of the evaluations + // we invoked. These are reset by a call to ResetTimeStatistics(). + double initial_evaluator_residual_time_in_seconds; + double initial_evaluator_jacobian_time_in_seconds; +}; + +// Backtracking and interpolation based Armijo line search. This +// implementation is based on the Armijo line search that ships in the +// minFunc package by Mark Schmidt. +// +// 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() {} + + private: + virtual void DoSearch(double step_size_estimate, + double initial_cost, + double initial_gradient, + Summary* summary) const; +}; + +// 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() {} + + // 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) const; + // 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) const; + + private: + virtual void DoSearch(double step_size_estimate, + double initial_cost, + double initial_gradient, + Summary* summary) const; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINE_SEARCH_H_ diff --git a/extern/ceres/internal/ceres/line_search_direction.cc b/extern/ceres/internal/ceres/line_search_direction.cc new file mode 100644 index 00000000000..1f9d205bff5 --- /dev/null +++ b/extern/ceres/internal/ceres/line_search_direction.cc @@ -0,0 +1,372 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/line_search_direction.h" +#include "ceres/line_search_minimizer.h" +#include "ceres/low_rank_inverse_hessian.h" +#include "ceres/internal/eigen.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +class SteepestDescent : public LineSearchDirection { + public: + virtual ~SteepestDescent() {} + bool NextDirection(const LineSearchMinimizer::State& previous, + const LineSearchMinimizer::State& current, + Vector* search_direction) { + *search_direction = -current.gradient; + return true; + } +}; + +class NonlinearConjugateGradient : public LineSearchDirection { + public: + NonlinearConjugateGradient(const NonlinearConjugateGradientType type, + const double function_tolerance) + : type_(type), + function_tolerance_(function_tolerance) { + } + + bool NextDirection(const LineSearchMinimizer::State& previous, + const LineSearchMinimizer::State& current, + Vector* search_direction) { + double beta = 0.0; + Vector gradient_change; + switch (type_) { + case FLETCHER_REEVES: + beta = current.gradient_squared_norm / previous.gradient_squared_norm; + break; + case POLAK_RIBIERE: + gradient_change = current.gradient - previous.gradient; + beta = (current.gradient.dot(gradient_change) / + previous.gradient_squared_norm); + break; + case HESTENES_STIEFEL: + gradient_change = current.gradient - previous.gradient; + beta = (current.gradient.dot(gradient_change) / + previous.search_direction.dot(gradient_change)); + break; + default: + LOG(FATAL) << "Unknown nonlinear conjugate gradient type: " << type_; + } + + *search_direction = -current.gradient + beta * previous.search_direction; + const double directional_derivative = + current.gradient.dot(*search_direction); + if (directional_derivative > -function_tolerance_) { + LOG(WARNING) << "Restarting non-linear conjugate gradients: " + << directional_derivative; + *search_direction = -current.gradient; + } + + return true; + } + + private: + const NonlinearConjugateGradientType type_; + const double function_tolerance_; +}; + +class LBFGS : public LineSearchDirection { + public: + 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); + + search_direction->setZero(); + 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); + + // The (L)BFGS algorithm explicitly requires that the secant equation: + // + // B_{k+1} * s_k = y_k + // + // Is satisfied at each iteration, where B_{k+1} is the approximated + // Hessian at the k+1-th iteration, s_k = (x_{k+1} - x_{k}) and + // y_k = (grad_{k+1} - grad_{k}). As the approximated Hessian must be + // positive definite, this is equivalent to the condition: + // + // s_k^T * y_k > 0 [s_k^T * B_{k+1} * s_k = s_k^T * y_k > 0] + // + // This condition would always be satisfied if the function was strictly + // convex, alternatively, it is always satisfied provided that a Wolfe line + // search is used (even if the function is not strictly convex). See [1] + // (p138) for a proof. + // + // Although Ceres will always use a Wolfe line search when using (L)BFGS, + // practical implementation considerations mean that the line search + // may return a point that satisfies only the Armijo condition, and thus + // could violate the Secant equation. As such, we will only use a step + // to update the Hessian approximation if: + // + // s_k^T * y_k > tolerance + // + // It is important that tolerance is very small (and >=0), as otherwise we + // might skip the update too often and fail to capture important curvature + // information in the Hessian. For example going from 1e-10 -> 1e-14 + // improves the NIST benchmark score from 43/54 to 53/54. + // + // [1] Nocedal J, Wright S, Numerical Optimization, 2nd Ed. Springer, 1999. + // + // TODO(alexs.mac): Consider using Damped BFGS update instead of + // skipping update. + const double kBFGSSecantConditionHessianUpdateTolerance = 1e-14; + if (delta_x_dot_delta_gradient <= + kBFGSSecantConditionHessianUpdateTolerance) { + VLOG(2) << "Skipping BFGS Update, delta_x_dot_delta_gradient too " + << "small: " << delta_x_dot_delta_gradient << ", tolerance: " + << kBFGSSecantConditionHessianUpdateTolerance + << " (Secant condition)."; + } 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. + const double approximate_eigenvalue_scale = + delta_x_dot_delta_gradient / delta_gradient.dot(delta_gradient); + inverse_hessian_ *= approximate_eigenvalue_scale; + + VLOG(4) << "Applying approximate_eigenvalue_scale: " + << approximate_eigenvalue_scale << " to initial inverse " + << "Hessian approximation."; + } + 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* +LineSearchDirection::Create(const LineSearchDirection::Options& options) { + if (options.type == STEEPEST_DESCENT) { + return new SteepestDescent; + } + + if (options.type == NONLINEAR_CONJUGATE_GRADIENT) { + return new NonlinearConjugateGradient( + options.nonlinear_conjugate_gradient_type, + options.function_tolerance); + } + + if (options.type == ceres::LBFGS) { + 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; + return NULL; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/line_search_direction.h b/extern/ceres/internal/ceres/line_search_direction.h new file mode 100644 index 00000000000..467578d5f7c --- /dev/null +++ b/extern/ceres/internal/ceres/line_search_direction.h @@ -0,0 +1,72 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_LINE_SEARCH_DIRECTION_H_ +#define CERES_INTERNAL_LINE_SEARCH_DIRECTION_H_ + +#include "ceres/internal/eigen.h" +#include "ceres/line_search_minimizer.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class LineSearchDirection { + public: + struct Options { + Options() + : num_parameters(0), + type(LBFGS), + nonlinear_conjugate_gradient_type(FLETCHER_REEVES), + function_tolerance(1e-12), + max_lbfgs_rank(20), + use_approximate_eigenvalue_bfgs_scaling(true) { + } + + int num_parameters; + LineSearchDirectionType type; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; + double function_tolerance; + int max_lbfgs_rank; + bool use_approximate_eigenvalue_bfgs_scaling; + }; + + static LineSearchDirection* Create(const Options& options); + + virtual ~LineSearchDirection() {} + virtual bool NextDirection(const LineSearchMinimizer::State& previous, + const LineSearchMinimizer::State& current, + Vector* search_direction) = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINE_SEARCH_DIRECTION_H_ diff --git a/extern/ceres/internal/ceres/line_search_minimizer.cc b/extern/ceres/internal/ceres/line_search_minimizer.cc new file mode 100644 index 00000000000..62264fb0b64 --- /dev/null +++ b/extern/ceres/internal/ceres/line_search_minimizer.cc @@ -0,0 +1,432 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// Generic loop for line search based optimization algorithms. +// +// This is primarily inpsired by the minFunc packaged written by Mark +// Schmidt. +// +// http://www.di.ens.fr/~mschmidt/Software/minFunc.html +// +// For details on the theory and implementation see "Numerical +// Optimization" by Nocedal & Wright. + +#include "ceres/line_search_minimizer.h" + +#include <algorithm> +#include <cstdlib> +#include <cmath> +#include <string> +#include <vector> + +#include "Eigen/Dense" +#include "ceres/array_utils.h" +#include "ceres/evaluator.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/line_search.h" +#include "ceres/line_search_direction.h" +#include "ceres/stringprintf.h" +#include "ceres/types.h" +#include "ceres/wall_time.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { + +// TODO(sameeragarwal): I think there is a small bug here, in that if +// the evaluation fails, then the state can contain garbage. Look at +// this more carefully. +bool Evaluate(Evaluator* evaluator, + const Vector& x, + LineSearchMinimizer::State* state, + std::string* message) { + if (!evaluator->Evaluate(x.data(), + &(state->cost), + NULL, + state->gradient.data(), + NULL)) { + *message = "Gradient evaluation failed."; + return false; + } + + Vector negative_gradient = -state->gradient; + Vector projected_gradient_step(x.size()); + if (!evaluator->Plus(x.data(), + negative_gradient.data(), + projected_gradient_step.data())) { + *message = "projected_gradient_step = Plus(x, -gradient) failed."; + return false; + } + + state->gradient_squared_norm = (x - projected_gradient_step).squaredNorm(); + state->gradient_max_norm = + (x - projected_gradient_step).lpNorm<Eigen::Infinity>(); + return true; +} + +} // namespace + +void LineSearchMinimizer::Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary) { + const bool is_not_silent = !options.is_silent; + double start_time = WallTimeInSeconds(); + double iteration_start_time = start_time; + + Evaluator* evaluator = CHECK_NOTNULL(options.evaluator.get()); + const int num_parameters = evaluator->NumParameters(); + const int num_effective_parameters = evaluator->NumEffectiveParameters(); + + summary->termination_type = NO_CONVERGENCE; + summary->num_successful_steps = 0; + summary->num_unsuccessful_steps = 0; + + VectorRef x(parameters, num_parameters); + + State current_state(num_parameters, num_effective_parameters); + State previous_state(num_parameters, num_effective_parameters); + + Vector delta(num_effective_parameters); + Vector x_plus_delta(num_parameters); + + IterationSummary iteration_summary; + iteration_summary.iteration = 0; + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; + iteration_summary.cost_change = 0.0; + iteration_summary.gradient_max_norm = 0.0; + iteration_summary.gradient_norm = 0.0; + iteration_summary.step_norm = 0.0; + iteration_summary.linear_solver_iterations = 0; + iteration_summary.step_solver_time_in_seconds = 0; + + // Do initial cost and Jacobian evaluation. + if (!Evaluate(evaluator, x, ¤t_state, &summary->message)) { + summary->termination_type = FAILURE; + summary->message = "Initial cost and jacobian evaluation failed. " + "More details: " + summary->message; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + return; + } + + summary->initial_cost = current_state.cost + summary->fixed_cost; + iteration_summary.cost = current_state.cost + summary->fixed_cost; + + iteration_summary.gradient_max_norm = current_state.gradient_max_norm; + iteration_summary.gradient_norm = sqrt(current_state.gradient_squared_norm); + + if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) { + summary->message = StringPrintf("Gradient tolerance reached. " + "Gradient max norm: %e <= %e", + iteration_summary.gradient_max_norm, + options.gradient_tolerance); + summary->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + return; + } + + iteration_summary.iteration_time_in_seconds = + WallTimeInSeconds() - iteration_start_time; + iteration_summary.cumulative_time_in_seconds = + WallTimeInSeconds() - start_time + + summary->preprocessor_time_in_seconds; + summary->iterations.push_back(iteration_summary); + + LineSearchDirection::Options line_search_direction_options; + line_search_direction_options.num_parameters = num_effective_parameters; + line_search_direction_options.type = options.line_search_direction_type; + 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; + + scoped_ptr<LineSearch> + line_search(LineSearch::Create(options.line_search_type, + line_search_options, + &summary->message)); + if (line_search.get() == NULL) { + summary->termination_type = FAILURE; + LOG_IF(ERROR, is_not_silent) << "Terminating: " << summary->message; + return; + } + + LineSearch::Summary line_search_summary; + int num_line_search_direction_restarts = 0; + + while (true) { + if (!RunCallbacks(options, iteration_summary, summary)) { + break; + } + + iteration_start_time = WallTimeInSeconds(); + if (iteration_summary.iteration >= options.max_num_iterations) { + summary->message = "Maximum number of iterations reached."; + summary->termination_type = NO_CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + break; + } + + const double total_solver_time = iteration_start_time - start_time + + summary->preprocessor_time_in_seconds; + if (total_solver_time >= options.max_solver_time_in_seconds) { + summary->message = "Maximum solver time reached."; + summary->termination_type = NO_CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + break; + } + + 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) { + current_state.search_direction = -current_state.gradient; + } else { + line_search_status = line_search_direction->NextDirection( + previous_state, + current_state, + ¤t_state.search_direction); + } + + 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->message = + StringPrintf("Line search direction failure: specified " + "max_num_line_search_direction_restarts: %d reached.", + options.max_num_line_search_direction_restarts); + summary->termination_type = FAILURE; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + 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_IF(WARNING, is_not_silent) + << "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; + } + + line_search_function.Init(x, current_state.search_direction); + current_state.directional_derivative = + current_state.gradient.dot(current_state.search_direction); + + // TODO(sameeragarwal): Refactor this into its own object and add + // explanations for the various choices. + // + // 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) + ? std::min(1.0, 1.0 / current_state.gradient_max_norm) + : std::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->message = + 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)); + summary->termination_type = FAILURE; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + break; + } + + line_search->Search(initial_step_size, + current_state.cost, + current_state.directional_derivative, + &line_search_summary); + if (!line_search_summary.success) { + summary->message = + StringPrintf("Numerical failure in line search, failed to find " + "a valid step size, (did not run out of iterations) " + "using initial_step_size: %.5e, initial_cost: %.5e, " + "initial_gradient: %.5e.", + initial_step_size, current_state.cost, + current_state.directional_derivative); + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + summary->termination_type = FAILURE; + break; + } + + 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; + + const double x_norm = x.norm(); + + if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) { + summary->termination_type = FAILURE; + summary->message = + "x_plus_delta = Plus(x, delta) failed. This should not happen " + "as the step was valid when it was selected by the line search."; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + break; + } else if (!Evaluate(evaluator, + x_plus_delta, + ¤t_state, + &summary->message)) { + summary->termination_type = FAILURE; + summary->message = + "Step failed to evaluate. This should not happen as the step was " + "valid when it was selected by the line search. More details: " + + summary->message; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + break; + } else { + x = x_plus_delta; + } + + iteration_summary.gradient_max_norm = current_state.gradient_max_norm; + iteration_summary.gradient_norm = sqrt(current_state.gradient_squared_norm); + iteration_summary.cost_change = previous_state.cost - current_state.cost; + iteration_summary.cost = current_state.cost + summary->fixed_cost; + iteration_summary.step_norm = delta.norm(); + iteration_summary.step_is_valid = true; + iteration_summary.step_is_successful = true; + iteration_summary.step_size = current_state.step_size; + iteration_summary.line_search_function_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 = + WallTimeInSeconds() - start_time + + summary->preprocessor_time_in_seconds; + + summary->line_search_cost_evaluation_time_in_seconds += + line_search_summary.cost_evaluation_time_in_seconds; + summary->line_search_gradient_evaluation_time_in_seconds += + line_search_summary.gradient_evaluation_time_in_seconds; + summary->line_search_polynomial_minimization_time_in_seconds += + line_search_summary.polynomial_minimization_time_in_seconds; + summary->line_search_total_time_in_seconds += + line_search_summary.total_time_in_seconds; + ++summary->num_successful_steps; + + const double step_size_tolerance = options.parameter_tolerance * + (x_norm + options.parameter_tolerance); + if (iteration_summary.step_norm <= step_size_tolerance) { + summary->message = + StringPrintf("Parameter tolerance reached. " + "Relative step_norm: %e <= %e.", + (iteration_summary.step_norm / + (x_norm + options.parameter_tolerance)), + options.parameter_tolerance); + summary->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + return; + } + + if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) { + summary->message = StringPrintf("Gradient tolerance reached. " + "Gradient max norm: %e <= %e", + iteration_summary.gradient_max_norm, + options.gradient_tolerance); + summary->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + break; + } + + const double absolute_function_tolerance = + options.function_tolerance * previous_state.cost; + if (fabs(iteration_summary.cost_change) <= absolute_function_tolerance) { + summary->message = + StringPrintf("Function tolerance reached. " + "|cost_change|/cost: %e <= %e", + fabs(iteration_summary.cost_change) / + previous_state.cost, + options.function_tolerance); + summary->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + break; + } + + summary->iterations.push_back(iteration_summary); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/line_search_minimizer.h b/extern/ceres/internal/ceres/line_search_minimizer.h new file mode 100644 index 00000000000..54b7202e0c3 --- /dev/null +++ b/extern/ceres/internal/ceres/line_search_minimizer.h @@ -0,0 +1,77 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_LINE_SEARCH_MINIMIZER_H_ +#define CERES_INTERNAL_LINE_SEARCH_MINIMIZER_H_ + +#include "ceres/minimizer.h" +#include "ceres/solver.h" +#include "ceres/types.h" +#include "ceres/internal/eigen.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Generic line search minimization algorithm. +// +// For example usage, see SolverImpl::Minimize. +class LineSearchMinimizer : public Minimizer { + public: + struct State { + State(int num_parameters, + int num_effective_parameters) + : cost(0.0), + gradient(num_effective_parameters), + gradient_squared_norm(0.0), + search_direction(num_effective_parameters), + directional_derivative(0.0), + step_size(0.0) { + } + + double cost; + Vector gradient; + double gradient_squared_norm; + double gradient_max_norm; + Vector search_direction; + double directional_derivative; + double step_size; + }; + + ~LineSearchMinimizer() {} + virtual void Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINE_SEARCH_MINIMIZER_H_ diff --git a/extern/ceres/internal/ceres/line_search_preprocessor.cc b/extern/ceres/internal/ceres/line_search_preprocessor.cc new file mode 100644 index 00000000000..831f5e8d079 --- /dev/null +++ b/extern/ceres/internal/ceres/line_search_preprocessor.cc @@ -0,0 +1,106 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/line_search_preprocessor.h" + +#include <numeric> +#include <string> +#include "ceres/evaluator.h" +#include "ceres/minimizer.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/wall_time.h" + +namespace ceres { +namespace internal { +namespace { + +bool IsProgramValid(const Program& program, std::string* error) { + if (program.IsBoundsConstrained()) { + *error = "LINE_SEARCH Minimizer does not support bounds."; + return false; + } + return program.ParameterBlocksAreFinite(error); +} + +bool SetupEvaluator(PreprocessedProblem* pp) { + pp->evaluator_options = Evaluator::Options(); + // This ensures that we get a Block Jacobian Evaluator without any + // requirement on orderings. + pp->evaluator_options.linear_solver_type = CGNR; + pp->evaluator_options.num_eliminate_blocks = 0; + pp->evaluator_options.num_threads = pp->options.num_threads; + pp->evaluator.reset(Evaluator::Create(pp->evaluator_options, + pp->reduced_program.get(), + &pp->error)); + return (pp->evaluator.get() != NULL); +} + +} // namespace + +LineSearchPreprocessor::~LineSearchPreprocessor() { +} + +bool LineSearchPreprocessor::Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* pp) { + CHECK_NOTNULL(pp); + pp->options = options; + ChangeNumThreadsIfNeeded(&pp->options); + + pp->problem = problem; + Program* program = problem->mutable_program(); + if (!IsProgramValid(*program, &pp->error)) { + return false; + } + + pp->reduced_program.reset( + program->CreateReducedProgram(&pp->removed_parameter_blocks, + &pp->fixed_cost, + &pp->error)); + + if (pp->reduced_program.get() == NULL) { + return false; + } + + if (pp->reduced_program->NumParameterBlocks() == 0) { + return true; + } + + if (!SetupEvaluator(pp)) { + return false; + } + + SetupCommonMinimizerOptions(pp); + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/line_search_preprocessor.h b/extern/ceres/internal/ceres/line_search_preprocessor.h new file mode 100644 index 00000000000..132d83a0a9a --- /dev/null +++ b/extern/ceres/internal/ceres/line_search_preprocessor.h @@ -0,0 +1,50 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: sameragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_LINE_SEARCH_PREPROCESSOR_H_ +#define CERES_INTERNAL_LINE_SEARCH_PREPROCESSOR_H_ + +#include "ceres/preprocessor.h" + +namespace ceres { +namespace internal { + +class LineSearchPreprocessor : public Preprocessor { + public: + virtual ~LineSearchPreprocessor(); + virtual bool Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* preprocessed_problem); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINE_SEARCH_PREPROCESSOR_H_ diff --git a/extern/ceres/internal/ceres/linear_least_squares_problems.cc b/extern/ceres/internal/ceres/linear_least_squares_problems.cc new file mode 100644 index 00000000000..0a69375f7b5 --- /dev/null +++ b/extern/ceres/internal/ceres/linear_least_squares_problems.cc @@ -0,0 +1,732 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_least_squares_problems.h" + +#include <cstdio> +#include <string> +#include <vector> +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/casts.h" +#include "ceres/file.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/stringprintf.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::string; + +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) { + switch (id) { + case 0: + return LinearLeastSquaresProblem0(); + case 1: + return LinearLeastSquaresProblem1(); + case 2: + return LinearLeastSquaresProblem2(); + case 3: + return LinearLeastSquaresProblem3(); + case 4: + return LinearLeastSquaresProblem4(); + default: + LOG(FATAL) << "Unknown problem id requested " << id; + } + return NULL; +} + +/* +A = [1 2] + [3 4] + [6 -10] + +b = [ 8 + 18 + -18] + +x = [2 + 3] + +D = [1 + 2] + +x_D = [1.78448275; + 2.82327586;] + */ +LinearLeastSquaresProblem* LinearLeastSquaresProblem0() { + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + TripletSparseMatrix* A = new TripletSparseMatrix(3, 2, 6); + problem->b.reset(new double[3]); + problem->D.reset(new double[2]); + + problem->x.reset(new double[2]); + problem->x_D.reset(new double[2]); + + int* Ai = A->mutable_rows(); + int* Aj = A->mutable_cols(); + double* Ax = A->mutable_values(); + + int counter = 0; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j< 2; ++j) { + Ai[counter] = i; + Aj[counter] = j; + ++counter; + } + } + + Ax[0] = 1.; + Ax[1] = 2.; + Ax[2] = 3.; + Ax[3] = 4.; + Ax[4] = 6; + Ax[5] = -10; + A->set_num_nonzeros(6); + problem->A.reset(A); + + problem->b[0] = 8; + problem->b[1] = 18; + problem->b[2] = -18; + + problem->x[0] = 2.0; + problem->x[1] = 3.0; + + problem->D[0] = 1; + problem->D[1] = 2; + + problem->x_D[0] = 1.78448275; + problem->x_D[1] = 2.82327586; + return problem; +} + + +/* + A = [1 0 | 2 0 0 + 3 0 | 0 4 0 + 0 5 | 0 0 6 + 0 7 | 8 0 0 + 0 9 | 1 0 0 + 0 0 | 1 1 1] + + b = [0 + 1 + 2 + 3 + 4 + 5] + + c = A'* b = [ 3 + 67 + 33 + 9 + 17] + + A'A = [10 0 2 12 0 + 0 155 65 0 30 + 2 65 70 1 1 + 12 0 1 17 1 + 0 30 1 1 37] + + S = [ 42.3419 -1.4000 -11.5806 + -1.4000 2.6000 1.0000 + 11.5806 1.0000 31.1935] + + r = [ 4.3032 + 5.4000 + 5.0323] + + S\r = [ 0.2102 + 2.1367 + 0.1388] + + A\b = [-2.3061 + 0.3172 + 0.2102 + 2.1367 + 0.1388] +*/ +// The following two functions create a TripletSparseMatrix and a +// BlockSparseMatrix version of this problem. + +// TripletSparseMatrix version. +LinearLeastSquaresProblem* LinearLeastSquaresProblem1() { + int num_rows = 6; + int num_cols = 5; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + TripletSparseMatrix* A = new TripletSparseMatrix(num_rows, + num_cols, + num_rows * num_cols); + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + int* rows = A->mutable_rows(); + int* cols = A->mutable_cols(); + double* values = A->mutable_values(); + + int nnz = 0; + + // Row 1 + { + rows[nnz] = 0; + cols[nnz] = 0; + values[nnz++] = 1; + + rows[nnz] = 0; + cols[nnz] = 2; + values[nnz++] = 2; + } + + // Row 2 + { + rows[nnz] = 1; + cols[nnz] = 0; + values[nnz++] = 3; + + rows[nnz] = 1; + cols[nnz] = 3; + values[nnz++] = 4; + } + + // Row 3 + { + rows[nnz] = 2; + cols[nnz] = 1; + values[nnz++] = 5; + + rows[nnz] = 2; + cols[nnz] = 4; + values[nnz++] = 6; + } + + // Row 4 + { + rows[nnz] = 3; + cols[nnz] = 1; + values[nnz++] = 7; + + rows[nnz] = 3; + cols[nnz] = 2; + values[nnz++] = 8; + } + + // Row 5 + { + rows[nnz] = 4; + cols[nnz] = 1; + values[nnz++] = 9; + + rows[nnz] = 4; + cols[nnz] = 2; + values[nnz++] = 1; + } + + // Row 6 + { + rows[nnz] = 5; + cols[nnz] = 2; + values[nnz++] = 1; + + rows[nnz] = 5; + cols[nnz] = 3; + values[nnz++] = 1; + + rows[nnz] = 5; + cols[nnz] = 4; + values[nnz++] = 1; + } + + A->set_num_nonzeros(nnz); + CHECK(A->IsValid()); + + problem->A.reset(A); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + return problem; +} + +// BlockSparseMatrix version +LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { + int num_rows = 6; + int num_cols = 5; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + scoped_array<double> values(new double[num_rows * num_cols]); + + for (int c = 0; c < num_cols; ++c) { + bs->cols.push_back(Block()); + bs->cols.back().size = 1; + bs->cols.back().position = c; + } + + int nnz = 0; + + // Row 1 + { + values[nnz++] = 1; + values[nnz++] = 2; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 0; + row.cells.push_back(Cell(0, 0)); + row.cells.push_back(Cell(2, 1)); + } + + // Row 2 + { + values[nnz++] = 3; + values[nnz++] = 4; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 1; + row.cells.push_back(Cell(0, 2)); + row.cells.push_back(Cell(3, 3)); + } + + // Row 3 + { + values[nnz++] = 5; + values[nnz++] = 6; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 2; + row.cells.push_back(Cell(1, 4)); + row.cells.push_back(Cell(4, 5)); + } + + // Row 4 + { + values[nnz++] = 7; + values[nnz++] = 8; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 3; + row.cells.push_back(Cell(1, 6)); + row.cells.push_back(Cell(2, 7)); + } + + // Row 5 + { + values[nnz++] = 9; + values[nnz++] = 1; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 4; + row.cells.push_back(Cell(1, 8)); + row.cells.push_back(Cell(2, 9)); + } + + // Row 6 + { + values[nnz++] = 1; + values[nnz++] = 1; + values[nnz++] = 1; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 5; + row.cells.push_back(Cell(2, 10)); + row.cells.push_back(Cell(3, 11)); + row.cells.push_back(Cell(4, 12)); + } + + BlockSparseMatrix* A = new BlockSparseMatrix(bs); + memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + problem->A.reset(A); + + return problem; +} + + +/* + A = [1 0 + 3 0 + 0 5 + 0 7 + 0 9 + 0 0] + + b = [0 + 1 + 2 + 3 + 4 + 5] +*/ +// BlockSparseMatrix version +LinearLeastSquaresProblem* LinearLeastSquaresProblem3() { + int num_rows = 5; + int num_cols = 2; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + scoped_array<double> values(new double[num_rows * num_cols]); + + for (int c = 0; c < num_cols; ++c) { + bs->cols.push_back(Block()); + bs->cols.back().size = 1; + bs->cols.back().position = c; + } + + int nnz = 0; + + // Row 1 + { + values[nnz++] = 1; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 0; + row.cells.push_back(Cell(0, 0)); + } + + // Row 2 + { + values[nnz++] = 3; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 1; + row.cells.push_back(Cell(0, 1)); + } + + // Row 3 + { + values[nnz++] = 5; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 2; + row.cells.push_back(Cell(1, 2)); + } + + // Row 4 + { + values[nnz++] = 7; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 3; + row.cells.push_back(Cell(1, 3)); + } + + // Row 5 + { + values[nnz++] = 9; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 4; + row.cells.push_back(Cell(1, 4)); + } + + BlockSparseMatrix* A = new BlockSparseMatrix(bs); + memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + problem->A.reset(A); + + return problem; +} + +/* + A = [1 2 0 0 0 1 1 + 1 4 0 0 0 5 6 + 0 0 9 0 0 3 1] + + b = [0 + 1 + 2] +*/ +// BlockSparseMatrix version +// +// This problem has the unique property that it has two different +// sized f-blocks, but only one of them occurs in the rows involving +// the one e-block. So performing Schur elimination on this problem +// tests the Schur Eliminator's ability to handle non-e-block rows +// correctly when their structure does not conform to the static +// structure determined by DetectStructure. +// +// NOTE: This problem is too small and rank deficient to be solved without +// the diagonal regularization. +LinearLeastSquaresProblem* LinearLeastSquaresProblem4() { + int num_rows = 3; + int num_cols = 7; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 1; + + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + scoped_array<double> values(new double[num_rows * num_cols]); + + // Column block structure + bs->cols.push_back(Block()); + bs->cols.back().size = 2; + bs->cols.back().position = 0; + + bs->cols.push_back(Block()); + bs->cols.back().size = 3; + bs->cols.back().position = 2; + + bs->cols.push_back(Block()); + bs->cols.back().size = 2; + bs->cols.back().position = 5; + + int nnz = 0; + + // Row 1 & 2 + { + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 2; + row.block.position = 0; + + row.cells.push_back(Cell(0, nnz)); + values[nnz++] = 1; + values[nnz++] = 2; + values[nnz++] = 1; + values[nnz++] = 4; + + row.cells.push_back(Cell(2, nnz)); + values[nnz++] = 1; + values[nnz++] = 1; + values[nnz++] = 5; + values[nnz++] = 6; + } + + // Row 3 + { + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 2; + + row.cells.push_back(Cell(1, nnz)); + values[nnz++] = 9; + values[nnz++] = 0; + values[nnz++] = 0; + + row.cells.push_back(Cell(2, nnz)); + values[nnz++] = 3; + values[nnz++] = 1; + } + + BlockSparseMatrix* A = new BlockSparseMatrix(bs); + memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = (i + 1) * 100; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + problem->A.reset(A); + return problem; +} + +namespace { +bool DumpLinearLeastSquaresProblemToConsole(const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + CHECK_NOTNULL(A); + Matrix AA; + A->ToDenseMatrix(&AA); + LOG(INFO) << "A^T: \n" << AA.transpose(); + + if (D != NULL) { + LOG(INFO) << "A's appended diagonal:\n" + << ConstVectorRef(D, A->num_cols()); + } + + if (b != NULL) { + LOG(INFO) << "b: \n" << ConstVectorRef(b, A->num_rows()); + } + + if (x != NULL) { + LOG(INFO) << "x: \n" << ConstVectorRef(x, A->num_cols()); + } + return true; +} + +void WriteArrayToFileOrDie(const string& filename, + const double* x, + const int size) { + CHECK_NOTNULL(x); + VLOG(2) << "Writing array to: " << filename; + FILE* fptr = fopen(filename.c_str(), "w"); + CHECK_NOTNULL(fptr); + for (int i = 0; i < size; ++i) { + fprintf(fptr, "%17f\n", x[i]); + } + fclose(fptr); +} + +bool DumpLinearLeastSquaresProblemToTextFile(const string& filename_base, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + CHECK_NOTNULL(A); + LOG(INFO) << "writing to: " << filename_base << "*"; + + string matlab_script; + StringAppendF(&matlab_script, + "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_base + "_A.txt"; + FILE* fptr = fopen(filename.c_str(), "w"); + CHECK_NOTNULL(fptr); + A->ToTextFile(fptr); + fclose(fptr); + StringAppendF(&matlab_script, + "tmp = load('%s', '-ascii');\n", filename.c_str()); + StringAppendF( + &matlab_script, + "lsqp.A = sparse(tmp(:, 1) + 1, tmp(:, 2) + 1, tmp(:, 3), %d, %d);\n", + A->num_rows(), + A->num_cols()); + } + + + if (D != NULL) { + string filename = filename_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_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_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_base + ".m"; + WriteStringToFileOrDie(matlab_script, matlab_filename); + return true; +} +} // namespace + +bool DumpLinearLeastSquaresProblem(const string& filename_base, + DumpFormatType dump_format_type, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + switch (dump_format_type) { + case CONSOLE: + return DumpLinearLeastSquaresProblemToConsole(A, D, b, x, + num_eliminate_blocks); + case TEXTFILE: + return DumpLinearLeastSquaresProblemToTextFile(filename_base, + A, D, b, x, + num_eliminate_blocks); + default: + LOG(FATAL) << "Unknown DumpFormatType " << dump_format_type; + } + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/linear_least_squares_problems.h b/extern/ceres/internal/ceres/linear_least_squares_problems.h new file mode 100644 index 00000000000..384efb59a2b --- /dev/null +++ b/extern/ceres/internal/ceres/linear_least_squares_problems.h @@ -0,0 +1,85 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ +#define CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ + +#include <string> +#include <vector> +#include "ceres/sparse_matrix.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// Structure defining a linear least squares problem and if possible +// ground truth solutions. To be used by various LinearSolver tests. +struct LinearLeastSquaresProblem { + LinearLeastSquaresProblem() + : A(NULL), b(NULL), D(NULL), num_eliminate_blocks(0), + x(NULL), x_D(NULL) { + } + + scoped_ptr<SparseMatrix> A; + scoped_array<double> b; + scoped_array<double> D; + // If using the schur eliminator then how many of the variable + // blocks are e_type blocks. + int num_eliminate_blocks; + + // Solution to min_x |Ax - b|^2 + scoped_array<double> x; + // Solution to min_x |Ax - b|^2 + |Dx|^2 + scoped_array<double> x_D; +}; + +// Factories for linear least squares problem. +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id); + +LinearLeastSquaresProblem* LinearLeastSquaresProblem0(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem1(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem2(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem3(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem4(); + +// Write the linear least squares problem to disk. The exact format +// depends on dump_format_type. +bool DumpLinearLeastSquaresProblem(const std::string& filename_base, + DumpFormatType dump_format_type, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks); +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ diff --git a/extern/ceres/internal/ceres/linear_operator.cc b/extern/ceres/internal/ceres/linear_operator.cc new file mode 100644 index 00000000000..9d291bd3465 --- /dev/null +++ b/extern/ceres/internal/ceres/linear_operator.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_operator.h" + +namespace ceres { +namespace internal { + +LinearOperator::~LinearOperator() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/linear_operator.h b/extern/ceres/internal/ceres/linear_operator.h new file mode 100644 index 00000000000..6463fb5089a --- /dev/null +++ b/extern/ceres/internal/ceres/linear_operator.h @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Base classes for access to an linear operator. + +#ifndef CERES_INTERNAL_LINEAR_OPERATOR_H_ +#define CERES_INTERNAL_LINEAR_OPERATOR_H_ + +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// This is an abstract base class for linear operators. It supports +// access to size information and left and right multiply operators. +class LinearOperator { + public: + virtual ~LinearOperator(); + + // y = y + Ax; + virtual void RightMultiply(const double* x, double* y) const = 0; + // y = y + A'x; + virtual void LeftMultiply(const double* x, double* y) const = 0; + + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_OPERATOR_H_ diff --git a/extern/ceres/internal/ceres/linear_solver.cc b/extern/ceres/internal/ceres/linear_solver.cc new file mode 100644 index 00000000000..38e4625f747 --- /dev/null +++ b/extern/ceres/internal/ceres/linear_solver.cc @@ -0,0 +1,119 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_solver.h" + +#include "ceres/cgnr_solver.h" +#include "ceres/dense_normal_cholesky_solver.h" +#include "ceres/dense_qr_solver.h" +#include "ceres/iterative_schur_complement_solver.h" +#include "ceres/schur_complement_solver.h" +#include "ceres/sparse_normal_cholesky_solver.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +LinearSolver::~LinearSolver() { +} + +LinearSolverType LinearSolver::LinearSolverForZeroEBlocks( + LinearSolverType linear_solver_type) { + if (!IsSchurType(linear_solver_type)) { + return linear_solver_type; + } + + if (linear_solver_type == SPARSE_SCHUR) { + return SPARSE_NORMAL_CHOLESKY; + } + + if (linear_solver_type == DENSE_SCHUR) { + // TODO(sameeragarwal): This is probably not a great choice. + // Ideally, we should have a DENSE_NORMAL_CHOLESKY, that can take + // a BlockSparseMatrix as input. + return DENSE_QR; + } + + if (linear_solver_type == ITERATIVE_SCHUR) { + return CGNR; + } + + return linear_solver_type; +} + +LinearSolver* LinearSolver::Create(const LinearSolver::Options& options) { + switch (options.type) { + case CGNR: + return new CgnrSolver(options); + + case SPARSE_NORMAL_CHOLESKY: +#if defined(CERES_NO_SUITESPARSE) && \ + defined(CERES_NO_CXSPARSE) && \ + !defined(CERES_USE_EIGEN_SPARSE) + return NULL; +#else + return new SparseNormalCholeskySolver(options); +#endif + + case SPARSE_SCHUR: +#if defined(CERES_NO_SUITESPARSE) && \ + defined(CERES_NO_CXSPARSE) && \ + !defined(CERES_USE_EIGEN_SPARSE) + return NULL; +#else + return new SparseSchurComplementSolver(options); +#endif + + case DENSE_SCHUR: + return new DenseSchurComplementSolver(options); + + case ITERATIVE_SCHUR: + if (options.use_explicit_schur_complement) { + return new SparseSchurComplementSolver(options); + } else { + return new IterativeSchurComplementSolver(options); + } + + case DENSE_QR: + return new DenseQRSolver(options); + + case DENSE_NORMAL_CHOLESKY: + return new DenseNormalCholeskySolver(options); + + default: + LOG(FATAL) << "Unknown linear solver type :" + << options.type; + return NULL; // MSVC doesn't understand that LOG(FATAL) never returns. + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/linear_solver.h b/extern/ceres/internal/ceres/linear_solver.h new file mode 100644 index 00000000000..fb9332ca6e3 --- /dev/null +++ b/extern/ceres/internal/ceres/linear_solver.h @@ -0,0 +1,362 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Abstract interface for objects solving linear systems of various +// kinds. + +#ifndef CERES_INTERNAL_LINEAR_SOLVER_H_ +#define CERES_INTERNAL_LINEAR_SOLVER_H_ + +#include <cstddef> +#include <map> +#include <string> +#include <vector> +#include "ceres/block_sparse_matrix.h" +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/dense_sparse_matrix.h" +#include "ceres/execution_summary.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +enum LinearSolverTerminationType { + // Termination criterion was met. + LINEAR_SOLVER_SUCCESS, + + // Solver ran for max_num_iterations and terminated before the + // termination tolerance could be satisfied. + LINEAR_SOLVER_NO_CONVERGENCE, + + // Solver was terminated due to numerical problems, generally due to + // the linear system being poorly conditioned. + LINEAR_SOLVER_FAILURE, + + // Solver failed with a fatal error that cannot be recovered from, + // e.g. CHOLMOD ran out of memory when computing the symbolic or + // numeric factorization or an underlying library was called with + // the wrong arguments. + LINEAR_SOLVER_FATAL_ERROR +}; + + +class LinearOperator; + +// Abstract base class for objects that implement algorithms for +// solving linear systems +// +// Ax = b +// +// It is expected that a single instance of a LinearSolver object +// maybe used multiple times for solving multiple linear systems with +// the same sparsity structure. This allows them to cache and reuse +// information across solves. This means that calling Solve on the +// same LinearSolver instance with two different linear systems will +// result in undefined behaviour. +// +// Subclasses of LinearSolver use two structs to configure themselves. +// The Options struct configures the LinearSolver object for its +// lifetime. The PerSolveOptions struct is used to specify options for +// a particular Solve call. +class LinearSolver { + public: + struct Options { + Options() + : type(SPARSE_NORMAL_CHOLESKY), + preconditioner_type(JACOBI), + visibility_clustering_type(CANONICAL_VIEWS), + dense_linear_algebra_library_type(EIGEN), + sparse_linear_algebra_library_type(SUITE_SPARSE), + use_postordering(false), + dynamic_sparsity(false), + use_explicit_schur_complement(false), + min_num_iterations(1), + max_num_iterations(1), + num_threads(1), + residual_reset_period(10), + row_block_size(Eigen::Dynamic), + e_block_size(Eigen::Dynamic), + f_block_size(Eigen::Dynamic) { + } + + LinearSolverType type; + PreconditionerType preconditioner_type; + VisibilityClusteringType visibility_clustering_type; + DenseLinearAlgebraLibraryType dense_linear_algebra_library_type; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; + + // See solver.h for information about these flags. + bool use_postordering; + bool dynamic_sparsity; + bool use_explicit_schur_complement; + + // Number of internal iterations that the solver uses. This + // parameter only makes sense for iterative solvers like CG. + int min_num_iterations; + int max_num_iterations; + + // If possible, how many threads can the solver use. + int num_threads; + + // Hints about the order in which the parameter blocks should be + // eliminated by the linear solver. + // + // For example if elimination_groups is a vector of size k, then + // the linear solver is informed that it should eliminate the + // parameter blocks 0 ... elimination_groups[0] - 1 first, and + // then elimination_groups[0] ... elimination_groups[1] - 1 and so + // on. Within each elimination group, the linear solver is free to + // choose how the parameter blocks are ordered. Different linear + // solvers have differing requirements on elimination_groups. + // + // The most common use is for Schur type solvers, where there + // should be at least two elimination groups and the first + // elimination group must form an independent set in the normal + // equations. The first elimination group corresponds to the + // num_eliminate_blocks in the Schur type solvers. + std::vector<int> elimination_groups; + + // Iterative solvers, e.g. Preconditioned Conjugate Gradients + // maintain a cheap estimate of the residual which may become + // inaccurate over time. Thus for non-zero values of this + // parameter, the solver can be told to recalculate the value of + // the residual using a |b - Ax| evaluation. + int residual_reset_period; + + // If the block sizes in a BlockSparseMatrix are fixed, then in + // some cases the Schur complement based solvers can detect and + // specialize on them. + // + // It is expected that these parameters are set programmatically + // rather than manually. + // + // Please see schur_complement_solver.h and schur_eliminator.h for + // more details. + int row_block_size; + int e_block_size; + int f_block_size; + }; + + // Options for the Solve method. + struct PerSolveOptions { + PerSolveOptions() + : D(NULL), + preconditioner(NULL), + r_tolerance(0.0), + q_tolerance(0.0) { + } + + // This option only makes sense for unsymmetric linear solvers + // that can solve rectangular linear systems. + // + // Given a matrix A, an optional diagonal matrix D as a vector, + // and a vector b, the linear solver will solve for + // + // | A | x = | b | + // | D | | 0 | + // + // If D is null, then it is treated as zero, and the solver returns + // the solution to + // + // A x = b + // + // In either case, x is the vector that solves the following + // optimization problem. + // + // arg min_x ||Ax - b||^2 + ||Dx||^2 + // + // Here A is a matrix of size m x n, with full column rank. If A + // does not have full column rank, the results returned by the + // solver cannot be relied on. D, if it is not null is an array of + // size n. b is an array of size m and x is an array of size n. + double * D; + + // This option only makes sense for iterative solvers. + // + // In general the performance of an iterative linear solver + // depends on the condition number of the matrix A. For example + // the convergence rate of the conjugate gradients algorithm + // is proportional to the square root of the condition number. + // + // One particularly useful technique for improving the + // conditioning of a linear system is to precondition it. In its + // simplest form a preconditioner is a matrix M such that instead + // of solving Ax = b, we solve the linear system AM^{-1} y = b + // instead, where M is such that the condition number k(AM^{-1}) + // is smaller than the conditioner k(A). Given the solution to + // this system, x = M^{-1} y. The iterative solver takes care of + // the mechanics of solving the preconditioned system and + // returning the corrected solution x. The user only needs to + // supply a linear operator. + // + // A null preconditioner is equivalent to an identity matrix being + // used a preconditioner. + LinearOperator* preconditioner; + + + // The following tolerance related options only makes sense for + // iterative solvers. Direct solvers ignore them. + + // Solver terminates when + // + // |Ax - b| <= r_tolerance * |b|. + // + // This is the most commonly used termination criterion for + // iterative solvers. + double r_tolerance; + + // For PSD matrices A, let + // + // Q(x) = x'Ax - 2b'x + // + // be the cost of the quadratic function defined by A and b. Then, + // the solver terminates at iteration i if + // + // i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance. + // + // This termination criterion is more useful when using CG to + // solve the Newton step. This particular convergence test comes + // from Stephen Nash's work on truncated Newton + // methods. References: + // + // 1. Stephen G. Nash & Ariela Sofer, Assessing A Search + // Direction Within A Truncated Newton Method, Operation + // Research Letters 9(1990) 219-221. + // + // 2. Stephen G. Nash, A Survey of Truncated Newton Methods, + // Journal of Computational and Applied Mathematics, + // 124(1-2), 45-59, 2000. + // + double q_tolerance; + }; + + // Summary of a call to the Solve method. We should move away from + // the true/false method for determining solver success. We should + // let the summary object do the talking. + struct Summary { + Summary() + : residual_norm(0.0), + num_iterations(-1), + termination_type(LINEAR_SOLVER_FAILURE) { + } + + double residual_norm; + int num_iterations; + LinearSolverTerminationType termination_type; + std::string message; + }; + + // If the optimization problem is such that there are no remaining + // e-blocks, a Schur type linear solver cannot be used. If the + // linear solver is of Schur type, this function implements a policy + // to select an alternate nearest linear solver to the one selected + // by the user. The input linear_solver_type is returned otherwise. + static LinearSolverType LinearSolverForZeroEBlocks( + LinearSolverType linear_solver_type); + + virtual ~LinearSolver(); + + // Solve Ax = b. + virtual Summary Solve(LinearOperator* A, + const double* b, + const PerSolveOptions& per_solve_options, + double* x) = 0; + + // The following two methods return copies instead of references so + // that the base class implementation does not have to worry about + // life time issues. Further, these calls are not expected to be + // frequent or performance sensitive. + virtual std::map<std::string, int> CallStatistics() const { + return std::map<std::string, int>(); + } + + virtual std::map<std::string, double> TimeStatistics() const { + return std::map<std::string, double>(); + } + + // Factory + static LinearSolver* Create(const Options& options); +}; + +// This templated subclass of LinearSolver serves as a base class for +// other linear solvers that depend on the particular matrix layout of +// the underlying linear operator. For example some linear solvers +// need low level access to the TripletSparseMatrix implementing the +// LinearOperator interface. This class hides those implementation +// details behind a private virtual method, and has the Solve method +// perform the necessary upcasting. +template <typename MatrixType> +class TypedLinearSolver : public LinearSolver { + public: + virtual ~TypedLinearSolver() {} + virtual LinearSolver::Summary Solve( + LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + ScopedExecutionTimer total_time("LinearSolver::Solve", &execution_summary_); + CHECK_NOTNULL(A); + CHECK_NOTNULL(b); + CHECK_NOTNULL(x); + return SolveImpl(down_cast<MatrixType*>(A), b, per_solve_options, x); + } + + virtual std::map<std::string, int> CallStatistics() const { + return execution_summary_.calls(); + } + + virtual std::map<std::string, double> TimeStatistics() const { + return execution_summary_.times(); + } + + private: + virtual LinearSolver::Summary SolveImpl( + MatrixType* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) = 0; + + ExecutionSummary execution_summary_; +}; + +// Linear solvers that depend on acccess to the low level structure of +// a SparseMatrix. +typedef TypedLinearSolver<BlockSparseMatrix> BlockSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<CompressedRowSparseMatrix> CompressedRowSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<DenseSparseMatrix> DenseSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<TripletSparseMatrix> TripletSparseMatrixSolver; // NOLINT + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/local_parameterization.cc b/extern/ceres/internal/ceres/local_parameterization.cc new file mode 100644 index 00000000000..82004761ec0 --- /dev/null +++ b/extern/ceres/internal/ceres/local_parameterization.cc @@ -0,0 +1,345 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/local_parameterization.h" + +#include "ceres/householder_vector.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/rotation.h" +#include "glog/logging.h" + +namespace ceres { + +using std::vector; + +LocalParameterization::~LocalParameterization() { +} + +bool LocalParameterization::MultiplyByJacobian(const double* x, + const int num_rows, + const double* global_matrix, + double* local_matrix) const { + Matrix jacobian(GlobalSize(), LocalSize()); + if (!ComputeJacobian(x, jacobian.data())) { + return false; + } + + MatrixRef(local_matrix, num_rows, LocalSize()) = + ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian; + return true; +} + +IdentityParameterization::IdentityParameterization(const int size) + : size_(size) { + CHECK_GT(size, 0); +} + +bool IdentityParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + VectorRef(x_plus_delta, size_) = + ConstVectorRef(x, size_) + ConstVectorRef(delta, size_); + return true; +} + +bool IdentityParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + MatrixRef(jacobian, size_, size_) = Matrix::Identity(size_, size_); + return true; +} + +bool IdentityParameterization::MultiplyByJacobian(const double* x, + const int num_cols, + const double* global_matrix, + double* local_matrix) const { + std::copy(global_matrix, + global_matrix + num_cols * GlobalSize(), + local_matrix); + return true; +} + +SubsetParameterization::SubsetParameterization( + int size, + const vector<int>& constant_parameters) + : local_size_(size - constant_parameters.size()), + constancy_mask_(size, 0) { + CHECK_GT(constant_parameters.size(), 0) + << "The set of constant parameters should contain at least " + << "one element. If you do not wish to hold any parameters " + << "constant, then do not use a SubsetParameterization"; + + vector<int> constant = constant_parameters; + sort(constant.begin(), constant.end()); + CHECK(unique(constant.begin(), constant.end()) == constant.end()) + << "The set of constant parameters cannot contain duplicates"; + CHECK_LT(constant_parameters.size(), size) + << "Number of parameters held constant should be less " + << "than the size of the parameter block. If you wish " + << "to hold the entire parameter block constant, then a " + << "efficient way is to directly mark it as constant " + << "instead of using a LocalParameterization to do so."; + CHECK_GE(*min_element(constant.begin(), constant.end()), 0); + CHECK_LT(*max_element(constant.begin(), constant.end()), size); + + for (int i = 0; i < constant_parameters.size(); ++i) { + constancy_mask_[constant_parameters[i]] = 1; + } +} + +bool SubsetParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { + if (constancy_mask_[i]) { + x_plus_delta[i] = x[i]; + } else { + x_plus_delta[i] = x[i] + delta[j++]; + } + } + return true; +} + +bool SubsetParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + MatrixRef m(jacobian, constancy_mask_.size(), local_size_); + m.setZero(); + for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { + if (!constancy_mask_[i]) { + m(i, j++) = 1.0; + } + } + return true; +} + +bool SubsetParameterization::MultiplyByJacobian(const double* x, + const int num_rows, + const double* global_matrix, + double* local_matrix) const { + for (int row = 0; row < num_rows; ++row) { + for (int col = 0, j = 0; col < constancy_mask_.size(); ++col) { + if (!constancy_mask_[col]) { + local_matrix[row * LocalSize() + j++] = + global_matrix[row * GlobalSize() + col]; + } + } + } + return true; +} + +bool QuaternionParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + const double norm_delta = + sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + if (norm_delta > 0.0) { + const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); + double q_delta[4]; + q_delta[0] = cos(norm_delta); + q_delta[1] = sin_delta_by_delta * delta[0]; + q_delta[2] = sin_delta_by_delta * delta[1]; + q_delta[3] = sin_delta_by_delta * delta[2]; + QuaternionProduct(q_delta, x, x_plus_delta); + } else { + for (int i = 0; i < 4; ++i) { + x_plus_delta[i] = x[i]; + } + } + return true; +} + +bool QuaternionParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; // NOLINT + jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; // NOLINT + jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; // NOLINT + jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; // NOLINT + return true; +} + +HomogeneousVectorParameterization::HomogeneousVectorParameterization(int size) + : size_(size) { + CHECK_GT(size_, 1) << "The size of the homogeneous vector needs to be " + << "greater than 1."; +} + +bool HomogeneousVectorParameterization::Plus(const double* x_ptr, + const double* delta_ptr, + double* x_plus_delta_ptr) const { + ConstVectorRef x(x_ptr, size_); + ConstVectorRef delta(delta_ptr, size_ - 1); + VectorRef x_plus_delta(x_plus_delta_ptr, size_); + + const double norm_delta = delta.norm(); + + if (norm_delta == 0.0) { + x_plus_delta = x; + return true; + } + + // Map the delta from the minimum representation to the over parameterized + // homogeneous vector. See section A6.9.2 on page 624 of Hartley & Zisserman + // (2nd Edition) for a detailed description. Note there is a typo on Page + // 625, line 4 so check the book errata. + const double norm_delta_div_2 = 0.5 * norm_delta; + const double sin_delta_by_delta = sin(norm_delta_div_2) / + norm_delta_div_2; + + Vector y(size_); + y.head(size_ - 1) = 0.5 * sin_delta_by_delta * delta; + y(size_ - 1) = cos(norm_delta_div_2); + + Vector v(size_); + double beta; + internal::ComputeHouseholderVector<double>(x, &v, &beta); + + // Apply the delta update to remain on the unit sphere. See section A6.9.3 + // on page 625 of Hartley & Zisserman (2nd Edition) for a detailed + // description. + x_plus_delta = x.norm() * (y - v * (beta * (v.transpose() * y))); + + return true; +} + +bool HomogeneousVectorParameterization::ComputeJacobian( + const double* x_ptr, double* jacobian_ptr) const { + ConstVectorRef x(x_ptr, size_); + MatrixRef jacobian(jacobian_ptr, size_, size_ - 1); + + Vector v(size_); + double beta; + internal::ComputeHouseholderVector<double>(x, &v, &beta); + + // The Jacobian is equal to J = 0.5 * H.leftCols(size_ - 1) where H is the + // Householder matrix (H = I - beta * v * v'). + for (int i = 0; i < size_ - 1; ++i) { + jacobian.col(i) = -0.5 * beta * v(i) * v; + jacobian.col(i)(i) += 0.5; + } + jacobian *= x.norm(); + + return true; +} + +ProductParameterization::ProductParameterization( + LocalParameterization* local_param1, + LocalParameterization* local_param2) { + local_params_.push_back(local_param1); + local_params_.push_back(local_param2); + Init(); +} + +ProductParameterization::ProductParameterization( + LocalParameterization* local_param1, + LocalParameterization* local_param2, + LocalParameterization* local_param3) { + local_params_.push_back(local_param1); + local_params_.push_back(local_param2); + local_params_.push_back(local_param3); + Init(); +} + +ProductParameterization::ProductParameterization( + LocalParameterization* local_param1, + LocalParameterization* local_param2, + LocalParameterization* local_param3, + LocalParameterization* local_param4) { + local_params_.push_back(local_param1); + local_params_.push_back(local_param2); + local_params_.push_back(local_param3); + local_params_.push_back(local_param4); + Init(); +} + +ProductParameterization::~ProductParameterization() { + for (int i = 0; i < local_params_.size(); ++i) { + delete local_params_[i]; + } +} + +void ProductParameterization::Init() { + global_size_ = 0; + local_size_ = 0; + buffer_size_ = 0; + for (int i = 0; i < local_params_.size(); ++i) { + const LocalParameterization* param = local_params_[i]; + buffer_size_ = std::max(buffer_size_, + param->LocalSize() * param->GlobalSize()); + global_size_ += param->GlobalSize(); + local_size_ += param->LocalSize(); + } +} + +bool ProductParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + int x_cursor = 0; + int delta_cursor = 0; + for (int i = 0; i < local_params_.size(); ++i) { + const LocalParameterization* param = local_params_[i]; + if (!param->Plus(x + x_cursor, + delta + delta_cursor, + x_plus_delta + x_cursor)) { + return false; + } + delta_cursor += param->LocalSize(); + x_cursor += param->GlobalSize(); + } + + return true; +} + +bool ProductParameterization::ComputeJacobian(const double* x, + double* jacobian_ptr) const { + MatrixRef jacobian(jacobian_ptr, GlobalSize(), LocalSize()); + jacobian.setZero(); + internal::FixedArray<double> buffer(buffer_size_); + + int x_cursor = 0; + int delta_cursor = 0; + for (int i = 0; i < local_params_.size(); ++i) { + const LocalParameterization* param = local_params_[i]; + const int local_size = param->LocalSize(); + const int global_size = param->GlobalSize(); + + if (!param->ComputeJacobian(x + x_cursor, buffer.get())) { + return false; + } + + jacobian.block(x_cursor, delta_cursor, global_size, local_size) + = MatrixRef(buffer.get(), global_size, local_size); + delta_cursor += local_size; + x_cursor += global_size; + } + + return true; +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/loss_function.cc b/extern/ceres/internal/ceres/loss_function.cc new file mode 100644 index 00000000000..eb5026784dd --- /dev/null +++ b/extern/ceres/internal/ceres/loss_function.cc @@ -0,0 +1,174 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Purpose: See .h file. + +#include "ceres/loss_function.h" + +#include <cmath> +#include <cstddef> +#include <limits> + +namespace ceres { + +void TrivialLoss::Evaluate(double s, double rho[3]) const { + rho[0] = s; + rho[1] = 1.0; + rho[2] = 0.0; +} + +void HuberLoss::Evaluate(double s, double rho[3]) const { + if (s > b_) { + // Outlier region. + // 'r' is always positive. + const double r = sqrt(s); + rho[0] = 2.0 * a_ * r - b_; + rho[1] = std::max(std::numeric_limits<double>::min(), a_ / r); + rho[2] = - rho[1] / (2.0 * s); + } else { + // Inlier region. + rho[0] = s; + rho[1] = 1.0; + rho[2] = 0.0; + } +} + +void SoftLOneLoss::Evaluate(double s, double rho[3]) const { + const double sum = 1.0 + s * c_; + const double tmp = sqrt(sum); + // 'sum' and 'tmp' are always positive, assuming that 's' is. + rho[0] = 2.0 * b_ * (tmp - 1.0); + rho[1] = std::max(std::numeric_limits<double>::min(), 1.0 / tmp); + rho[2] = - (c_ * rho[1]) / (2.0 * sum); +} + +void CauchyLoss::Evaluate(double s, double rho[3]) const { + const double sum = 1.0 + s * c_; + const double inv = 1.0 / sum; + // 'sum' and 'inv' are always positive, assuming that 's' is. + rho[0] = b_ * log(sum); + rho[1] = std::max(std::numeric_limits<double>::min(), inv); + rho[2] = - c_ * (inv * inv); +} + +void ArctanLoss::Evaluate(double s, double rho[3]) const { + const double sum = 1 + s * s * b_; + const double inv = 1 / sum; + // 'sum' and 'inv' are always positive. + rho[0] = a_ * atan2(s, a_); + rho[1] = std::max(std::numeric_limits<double>::min(), inv); + rho[2] = -2.0 * s * b_ * (inv * inv); +} + +TolerantLoss::TolerantLoss(double a, double b) + : a_(a), + b_(b), + c_(b * log(1.0 + exp(-a / b))) { + CHECK_GE(a, 0.0); + CHECK_GT(b, 0.0); +} + +void TolerantLoss::Evaluate(double s, double rho[3]) const { + const double x = (s - a_) / b_; + // The basic equation is rho[0] = b ln(1 + e^x). However, if e^x is too + // large, it will overflow. Since numerically 1 + e^x == e^x when the + // x is greater than about ln(2^53) for doubles, beyond this threshold + // we substitute x for ln(1 + e^x) as a numerically equivalent approximation. + static const double kLog2Pow53 = 36.7; // ln(MathLimits<double>::kEpsilon). + if (x > kLog2Pow53) { + rho[0] = s - a_ - c_; + rho[1] = 1.0; + rho[2] = 0.0; + } else { + const double e_x = exp(x); + rho[0] = b_ * log(1.0 + e_x) - c_; + rho[1] = std::max(std::numeric_limits<double>::min(), e_x / (1.0 + e_x)); + rho[2] = 0.5 / (b_ * (1.0 + cosh(x))); + } +} + +void TukeyLoss::Evaluate(double s, double* rho) const { + if (s <= a_squared_) { + // Inlier region. + const double value = 1.0 - s / a_squared_; + const double value_sq = value * value; + rho[0] = a_squared_ / 6.0 * (1.0 - value_sq * value); + rho[1] = 0.5 * value_sq; + rho[2] = -1.0 / a_squared_ * value; + } else { + // Outlier region. + rho[0] = a_squared_ / 6.0; + rho[1] = 0.0; + rho[2] = 0.0; + } +} + +ComposedLoss::ComposedLoss(const LossFunction* f, Ownership ownership_f, + const LossFunction* g, Ownership ownership_g) + : f_(CHECK_NOTNULL(f)), + g_(CHECK_NOTNULL(g)), + ownership_f_(ownership_f), + ownership_g_(ownership_g) { +} + +ComposedLoss::~ComposedLoss() { + if (ownership_f_ == DO_NOT_TAKE_OWNERSHIP) { + f_.release(); + } + if (ownership_g_ == DO_NOT_TAKE_OWNERSHIP) { + g_.release(); + } +} + +void ComposedLoss::Evaluate(double s, double rho[3]) const { + double rho_f[3], rho_g[3]; + g_->Evaluate(s, rho_g); + f_->Evaluate(rho_g[0], rho_f); + rho[0] = rho_f[0]; + // f'(g(s)) * g'(s). + rho[1] = rho_f[1] * rho_g[1]; + // f''(g(s)) * g'(s) * g'(s) + f'(g(s)) * g''(s). + rho[2] = rho_f[2] * rho_g[1] * rho_g[1] + rho_f[1] * rho_g[2]; +} + +void ScaledLoss::Evaluate(double s, double rho[3]) const { + if (rho_.get() == NULL) { + rho[0] = a_ * s; + rho[1] = a_; + rho[2] = 0.0; + } else { + rho_->Evaluate(s, rho); + rho[0] *= a_; + rho[1] *= a_; + rho[2] *= a_; + } +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/low_rank_inverse_hessian.cc b/extern/ceres/internal/ceres/low_rank_inverse_hessian.cc new file mode 100644 index 00000000000..1c6c9925f1c --- /dev/null +++ b/extern/ceres/internal/ceres/low_rank_inverse_hessian.cc @@ -0,0 +1,188 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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 <list> + +#include "ceres/internal/eigen.h" +#include "ceres/low_rank_inverse_hessian.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::list; + +// The (L)BFGS algorithm explicitly requires that the secant equation: +// +// B_{k+1} * s_k = y_k +// +// Is satisfied at each iteration, where B_{k+1} is the approximated +// Hessian at the k+1-th iteration, s_k = (x_{k+1} - x_{k}) and +// y_k = (grad_{k+1} - grad_{k}). As the approximated Hessian must be +// positive definite, this is equivalent to the condition: +// +// s_k^T * y_k > 0 [s_k^T * B_{k+1} * s_k = s_k^T * y_k > 0] +// +// This condition would always be satisfied if the function was strictly +// convex, alternatively, it is always satisfied provided that a Wolfe line +// search is used (even if the function is not strictly convex). See [1] +// (p138) for a proof. +// +// Although Ceres will always use a Wolfe line search when using (L)BFGS, +// practical implementation considerations mean that the line search +// may return a point that satisfies only the Armijo condition, and thus +// could violate the Secant equation. As such, we will only use a step +// to update the Hessian approximation if: +// +// s_k^T * y_k > tolerance +// +// It is important that tolerance is very small (and >=0), as otherwise we +// might skip the update too often and fail to capture important curvature +// information in the Hessian. For example going from 1e-10 -> 1e-14 improves +// the NIST benchmark score from 43/54 to 53/54. +// +// [1] Nocedal J., Wright S., Numerical Optimization, 2nd Ed. Springer, 1999. +// +// TODO(alexs.mac): Consider using Damped BFGS update instead of +// skipping update. +const double kLBFGSSecantConditionHessianUpdateTolerance = 1e-14; + +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), + 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) { +} + +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 <= + kLBFGSSecantConditionHessianUpdateTolerance) { + VLOG(2) << "Skipping L-BFGS Update, delta_x_dot_delta_gradient too " + << "small: " << delta_x_dot_delta_gradient << ", tolerance: " + << kLBFGSSecantConditionHessianUpdateTolerance + << " (Secant condition)."; + return false; + } + + + int next = indices_.size(); + // Once the size of the list reaches max_num_corrections_, simulate + // a circular buffer by removing the first element of the list and + // making it the next position where the LBFGS history is stored. + if (next == max_num_corrections_) { + next = indices_.front(); + indices_.pop_front(); + } + + indices_.push_back(next); + delta_x_history_.col(next) = delta_x; + delta_gradient_history_.col(next) = delta_gradient; + delta_x_dot_delta_gradient_(next) = delta_x_dot_delta_gradient; + approximate_eigenvalue_scale_ = + delta_x_dot_delta_gradient / delta_gradient.squaredNorm(); + return true; +} + +void LowRankInverseHessian::RightMultiply(const double* x_ptr, + double* y_ptr) const { + ConstVectorRef gradient(x_ptr, num_parameters_); + VectorRef search_direction(y_ptr, num_parameters_); + + search_direction = gradient; + + const int num_corrections = indices_.size(); + Vector alpha(num_corrections); + + for (list<int>::const_reverse_iterator it = indices_.rbegin(); + it != indices_.rend(); + ++it) { + const double alpha_i = delta_x_history_.col(*it).dot(search_direction) / + delta_x_dot_delta_gradient_(*it); + search_direction -= alpha_i * delta_gradient_history_.col(*it); + alpha(*it) = alpha_i; + } + + 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_; + + VLOG(4) << "Applying approximate_eigenvalue_scale: " + << approximate_eigenvalue_scale_ << " to initial inverse Hessian " + << "approximation."; + } + + for (list<int>::const_iterator it = indices_.begin(); + it != indices_.end(); + ++it) { + const double beta = delta_gradient_history_.col(*it).dot(search_direction) / + delta_x_dot_delta_gradient_(*it); + search_direction += delta_x_history_.col(*it) * (alpha(*it) - beta); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/low_rank_inverse_hessian.h b/extern/ceres/internal/ceres/low_rank_inverse_hessian.h new file mode 100644 index 00000000000..2c768c2ca53 --- /dev/null +++ b/extern/ceres/internal/ceres/low_rank_inverse_hessian.h @@ -0,0 +1,108 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// Limited memory positive definite approximation to the inverse +// Hessian, using the LBFGS algorithm + +#ifndef CERES_INTERNAL_LOW_RANK_INVERSE_HESSIAN_H_ +#define CERES_INTERNAL_LOW_RANK_INVERSE_HESSIAN_H_ + +#include <list> + +#include "ceres/internal/eigen.h" +#include "ceres/linear_operator.h" + +namespace ceres { +namespace internal { + +// LowRankInverseHessian is a positive definite approximation to the +// Hessian using the limited memory variant of the +// Broyden-Fletcher-Goldfarb-Shanno (BFGS)secant formula for +// approximating the Hessian. +// +// Other update rules like the Davidon-Fletcher-Powell (DFP) are +// possible, but the BFGS rule is considered the best performing one. +// +// The limited memory variant was developed by Nocedal and further +// enhanced with scaling rule by Byrd, Nocedal and Schanbel. +// +// Nocedal, J. (1980). "Updating Quasi-Newton Matrices with Limited +// Storage". Mathematics of Computation 35 (151): 773–782. +// +// Byrd, R. H.; Nocedal, J.; Schnabel, R. B. (1994). +// "Representations of Quasi-Newton Matrices and their use in +// Limited Memory Methods". Mathematical Programming 63 (4): +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, + bool use_approximate_eigenvalue_scaling); + virtual ~LowRankInverseHessian() {} + + // Update the low rank approximation. delta_x is the change in the + // domain of Hessian, and delta_gradient is the change in the + // gradient. The update copies the delta_x and delta_gradient + // vectors, and gets rid of the oldest delta_x and delta_gradient + // vectors if the number of corrections is already equal to + // max_num_corrections. + bool Update(const Vector& delta_x, const Vector& delta_gradient); + + // LinearOperator interface + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + virtual int num_rows() const { return num_parameters_; } + virtual int num_cols() const { return num_parameters_; } + + private: + const int num_parameters_; + const int max_num_corrections_; + const bool use_approximate_eigenvalue_scaling_; + double approximate_eigenvalue_scale_; + ColMajorMatrix delta_x_history_; + ColMajorMatrix delta_gradient_history_; + Vector delta_x_dot_delta_gradient_; + std::list<int> indices_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LOW_RANK_INVERSE_HESSIAN_H_ diff --git a/extern/ceres/internal/ceres/map_util.h b/extern/ceres/internal/ceres/map_util.h new file mode 100644 index 00000000000..61c531f297c --- /dev/null +++ b/extern/ceres/internal/ceres/map_util.h @@ -0,0 +1,130 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Originally by Anton Carver + +#ifndef CERES_INTERNAL_MAP_UTIL_H_ +#define CERES_INTERNAL_MAP_UTIL_H_ + +#include <utility> +#include "ceres/internal/port.h" +#include "glog/logging.h" + +namespace ceres { + +// Perform a lookup in a map or hash_map, assuming that the key exists. +// Crash if it does not. +// +// This is intended as a replacement for operator[] as an rvalue (for reading) +// when the key is guaranteed to exist. +// +// operator[] is discouraged for several reasons: +// * It has a side-effect of inserting missing keys +// * It is not thread-safe (even when it is not inserting, it can still +// choose to resize the underlying storage) +// * It invalidates iterators (when it chooses to resize) +// * It default constructs a value object even if it doesn't need to +// +// This version assumes the key is printable, and includes it in the fatal log +// message. +template <class Collection> +const typename Collection::value_type::second_type& +FindOrDie(const Collection& collection, + const typename Collection::value_type::first_type& key) { + typename Collection::const_iterator it = collection.find(key); + CHECK(it != collection.end()) << "Map key not found: " << key; + return it->second; +} + +// Perform a lookup in a map or hash_map. +// If the key is present in the map then the value associated with that +// key is returned, otherwise the value passed as a default is returned. +template <class Collection> +const typename Collection::value_type::second_type& +FindWithDefault(const Collection& collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& value) { + typename Collection::const_iterator it = collection.find(key); + if (it == collection.end()) { + return value; + } + return it->second; +} + +// Insert a new key and value into a map or hash_map. +// If the key is not present in the map the key and value are +// inserted, otherwise nothing happens. True indicates that an insert +// took place, false indicates the key was already present. +template <class Collection> +bool InsertIfNotPresent( + Collection * const collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& value) { + std::pair<typename Collection::iterator, bool> ret = + collection->insert(typename Collection::value_type(key, value)); + return ret.second; +} + +// Perform a lookup in a map or hash_map. +// Same as above but the returned pointer is not const and can be used to change +// the stored value. +template <class Collection> +typename Collection::value_type::second_type* +FindOrNull(Collection& collection, // NOLINT + const typename Collection::value_type::first_type& key) { + typename Collection::iterator it = collection.find(key); + if (it == collection.end()) { + return 0; + } + return &it->second; +} + +// Test to see if a set, map, hash_set or hash_map contains a particular key. +// Returns true if the key is in the collection. +template <class Collection, class Key> +bool ContainsKey(const Collection& collection, const Key& key) { + typename Collection::const_iterator it = collection.find(key); + return it != collection.end(); +} + +// Inserts a new key/value into a map or hash_map. +// Dies if the key is already present. +template<class Collection> +void InsertOrDie(Collection* const collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& data) { + typedef typename Collection::value_type value_type; + CHECK(collection->insert(value_type(key, data)).second) + << "duplicate key: " << key; +} + +} // namespace ceres + +#endif // CERES_INTERNAL_MAP_UTIL_H_ diff --git a/extern/ceres/internal/ceres/minimizer.cc b/extern/ceres/internal/ceres/minimizer.cc new file mode 100644 index 00000000000..f5960336f12 --- /dev/null +++ b/extern/ceres/internal/ceres/minimizer.cc @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/line_search_minimizer.h" +#include "ceres/minimizer.h" +#include "ceres/trust_region_minimizer.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +Minimizer* Minimizer::Create(MinimizerType minimizer_type) { + if (minimizer_type == TRUST_REGION) { + return new TrustRegionMinimizer; + } + + if (minimizer_type == LINE_SEARCH) { + return new LineSearchMinimizer; + } + + LOG(FATAL) << "Unknown minimizer_type: " << minimizer_type; + return NULL; +} + + +Minimizer::~Minimizer() {} + +bool Minimizer::RunCallbacks(const Minimizer::Options& options, + const IterationSummary& iteration_summary, + Solver::Summary* summary) { + const bool is_not_silent = !options.is_silent; + CallbackReturnType status = SOLVER_CONTINUE; + int i = 0; + while (status == SOLVER_CONTINUE && i < options.callbacks.size()) { + status = (*options.callbacks[i])(iteration_summary); + ++i; + } + switch (status) { + case SOLVER_CONTINUE: + return true; + case SOLVER_TERMINATE_SUCCESSFULLY: + summary->termination_type = USER_SUCCESS; + summary->message = + "User callback returned SOLVER_TERMINATE_SUCCESSFULLY."; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + return false; + case SOLVER_ABORT: + summary->termination_type = USER_FAILURE; + summary->message = "User callback returned SOLVER_ABORT."; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + return false; + default: + LOG(FATAL) << "Unknown type of user callback status"; + } + return false; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/minimizer.h b/extern/ceres/internal/ceres/minimizer.h new file mode 100644 index 00000000000..b59372806e7 --- /dev/null +++ b/extern/ceres/internal/ceres/minimizer.h @@ -0,0 +1,202 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_MINIMIZER_H_ +#define CERES_INTERNAL_MINIMIZER_H_ + +#include <string> +#include <vector> +#include "ceres/internal/port.h" +#include "ceres/iteration_callback.h" +#include "ceres/solver.h" + +namespace ceres { +namespace internal { + +class Evaluator; +class SparseMatrix; +class TrustRegionStrategy; +class CoordinateDescentMinimizer; +class LinearSolver; + +// Interface for non-linear least squares solvers. +class Minimizer { + public: + // Options struct to control the behaviour of the Minimizer. Please + // see solver.h for detailed information about the meaning and + // default values of each of these parameters. + struct Options { + Options() { + Init(Solver::Options()); + } + + explicit Options(const Solver::Options& options) { + Init(options); + } + + void Init(const Solver::Options& options) { + num_threads = options.num_threads; + max_num_iterations = options.max_num_iterations; + max_solver_time_in_seconds = options.max_solver_time_in_seconds; + max_step_solver_retries = 5; + gradient_tolerance = options.gradient_tolerance; + parameter_tolerance = options.parameter_tolerance; + function_tolerance = options.function_tolerance; + min_relative_decrease = options.min_relative_decrease; + eta = options.eta; + jacobi_scaling = options.jacobi_scaling; + use_nonmonotonic_steps = options.use_nonmonotonic_steps; + max_consecutive_nonmonotonic_steps = + options.max_consecutive_nonmonotonic_steps; + 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; + line_search_direction_type = options.line_search_direction_type; + line_search_type = options.line_search_type; + 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; + inner_iteration_tolerance = options.inner_iteration_tolerance; + is_silent = (options.logging_type == SILENT); + is_constrained = false; + callbacks = options.callbacks; + } + + int max_num_iterations; + double max_solver_time_in_seconds; + int num_threads; + + // Number of times the linear solver should be retried in case of + // numerical failure. The retries are done by exponentially scaling up + // mu at each retry. This leads to stronger and stronger + // regularization making the linear least squares problem better + // conditioned at each retry. + int max_step_solver_retries; + double gradient_tolerance; + double parameter_tolerance; + double function_tolerance; + double min_relative_decrease; + double eta; + bool jacobi_scaling; + bool use_nonmonotonic_steps; + int max_consecutive_nonmonotonic_steps; + std::vector<int> trust_region_minimizer_iterations_to_dump; + DumpFormatType trust_region_problem_dump_format_type; + std::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; + double inner_iteration_tolerance; + + // If true, then all logging is disabled. + bool is_silent; + + // Use a bounds constrained optimization algorithm. + bool is_constrained; + + // List of callbacks that are executed by the Minimizer at the end + // of each iteration. + // + // The Options struct does not own these pointers. + std::vector<IterationCallback*> callbacks; + + // Object responsible for evaluating the cost, residuals and + // Jacobian matrix. + shared_ptr<Evaluator> evaluator; + + // Object responsible for actually computing the trust region + // step, and sizing the trust region radius. + shared_ptr<TrustRegionStrategy> trust_region_strategy; + + // Object holding the Jacobian matrix. It is assumed that the + // sparsity structure of the matrix has already been initialized + // and will remain constant for the life time of the + // optimization. + shared_ptr<SparseMatrix> jacobian; + + shared_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; + }; + + static Minimizer* Create(MinimizerType minimizer_type); + static bool RunCallbacks(const Options& options, + const IterationSummary& iteration_summary, + Solver::Summary* summary); + + virtual ~Minimizer(); + // Note: The minimizer is expected to update the state of the + // parameters array every iteration. This is required for the + // StateUpdatingCallback to work. + virtual void Minimize(const Options& options, + double* parameters, + Solver::Summary* summary) = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_MINIMIZER_H_ diff --git a/extern/ceres/internal/ceres/mutex.h b/extern/ceres/internal/ceres/mutex.h new file mode 100644 index 00000000000..2ce97772755 --- /dev/null +++ b/extern/ceres/internal/ceres/mutex.h @@ -0,0 +1,329 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Craig Silverstein. +// +// A simple mutex wrapper, supporting locks and read-write locks. +// You should assume the locks are *not* re-entrant. +// +// This class is meant to be internal-only and should be wrapped by an +// internal namespace. Before you use this module, please give the +// name of your internal namespace for this module. Or, if you want +// to expose it, you'll want to move it to the Google namespace. We +// cannot put this class in global namespace because there can be some +// problems when we have multiple versions of Mutex in each shared object. +// +// NOTE: by default, we have #ifdef'ed out the TryLock() method. +// This is for two reasons: +// 1) TryLock() under Windows is a bit annoying (it requires a +// #define to be defined very early). +// 2) TryLock() is broken for NO_THREADS mode, at least in NDEBUG +// mode. +// If you need TryLock(), and either these two caveats are not a +// problem for you, or you're willing to work around them, then +// feel free to #define GMUTEX_TRYLOCK, or to remove the #ifdefs +// in the code below. +// +// CYGWIN NOTE: Cygwin support for rwlock seems to be buggy: +// http://www.cygwin.com/ml/cygwin/2008-12/msg00017.html +// Because of that, we might as well use windows locks for +// cygwin. They seem to be more reliable than the cygwin pthreads layer. +// +// TRICKY IMPLEMENTATION NOTE: +// This class is designed to be safe to use during +// dynamic-initialization -- that is, by global constructors that are +// run before main() starts. The issue in this case is that +// dynamic-initialization happens in an unpredictable order, and it +// could be that someone else's dynamic initializer could call a +// function that tries to acquire this mutex -- but that all happens +// before this mutex's constructor has run. (This can happen even if +// the mutex and the function that uses the mutex are in the same .cc +// file.) Basically, because Mutex does non-trivial work in its +// constructor, it's not, in the naive implementation, safe to use +// before dynamic initialization has run on it. +// +// The solution used here is to pair the actual mutex primitive with a +// bool that is set to true when the mutex is dynamically initialized. +// (Before that it's false.) Then we modify all mutex routines to +// look at the bool, and not try to lock/unlock until the bool makes +// it to true (which happens after the Mutex constructor has run.) +// +// This works because before main() starts -- particularly, during +// dynamic initialization -- there are no threads, so a) it's ok that +// the mutex operations are a no-op, since we don't need locking then +// anyway; and b) we can be quite confident our bool won't change +// state between a call to Lock() and a call to Unlock() (that would +// require a global constructor in one translation unit to call Lock() +// and another global constructor in another translation unit to call +// Unlock() later, which is pretty perverse). +// +// That said, it's tricky, and can conceivably fail; it's safest to +// avoid trying to acquire a mutex in a global constructor, if you +// can. One way it can fail is that a really smart compiler might +// initialize the bool to true at static-initialization time (too +// early) rather than at dynamic-initialization time. To discourage +// that, we set is_safe_ to true in code (not the constructor +// colon-initializer) and set it to true via a function that always +// evaluates to true, but that the compiler can't know always +// evaluates to true. This should be good enough. + +#ifndef CERES_INTERNAL_MUTEX_H_ +#define CERES_INTERNAL_MUTEX_H_ + +#include "ceres/internal/port.h" + +#if defined(CERES_NO_THREADS) + typedef int MutexType; // to keep a lock-count +#elif defined(_WIN32) || defined(__CYGWIN32__) || defined(__CYGWIN64__) +# define CERES_WIN32_LEAN_AND_MEAN // We only need minimal includes +# ifdef CERES_GMUTEX_TRYLOCK + // We need Windows NT or later for TryEnterCriticalSection(). If you + // don't need that functionality, you can remove these _WIN32_WINNT + // lines, and change TryLock() to assert(0) or something. +# ifndef _WIN32_WINNT +# define _WIN32_WINNT 0x0400 +# endif +# endif +// Unfortunately, windows.h defines a bunch of macros with common +// names. Two in particular need avoiding: ERROR and min/max. +// To avoid macro definition of ERROR. +# define NOGDI +// To avoid macro definition of min/max. +# ifndef NOMINMAX +# define NOMINMAX +# endif +# include <windows.h> + typedef CRITICAL_SECTION MutexType; +#elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK) + // Needed for pthread_rwlock_*. If it causes problems, you could take it + // out, but then you'd have to unset CERES_HAVE_RWLOCK (at least on linux -- + // it *does* cause problems for FreeBSD, or MacOSX, but isn't needed for + // locking there.) +# if defined(__linux__) && !defined(_XOPEN_SOURCE) +# define _XOPEN_SOURCE 500 // may be needed to get the rwlock calls +# endif +# include <pthread.h> + typedef pthread_rwlock_t MutexType; +#elif defined(CERES_HAVE_PTHREAD) +# include <pthread.h> + typedef pthread_mutex_t MutexType; +#else +# error Need to implement mutex.h for your architecture, or #define NO_THREADS +#endif + +// We need to include these header files after defining _XOPEN_SOURCE +// as they may define the _XOPEN_SOURCE macro. +#include <assert.h> +#include <stdlib.h> // for abort() + +namespace ceres { +namespace internal { + +class Mutex { + public: + // Create a Mutex that is not held by anybody. This constructor is + // typically used for Mutexes allocated on the heap or the stack. + // See below for a recommendation for constructing global Mutex + // objects. + inline Mutex(); + + // Destructor + inline ~Mutex(); + + inline void Lock(); // Block if needed until free then acquire exclusively + inline void Unlock(); // Release a lock acquired via Lock() +#ifdef CERES_GMUTEX_TRYLOCK + inline bool TryLock(); // If free, Lock() and return true, else return false +#endif + // Note that on systems that don't support read-write locks, these may + // be implemented as synonyms to Lock() and Unlock(). So you can use + // these for efficiency, but don't use them anyplace where being able + // to do shared reads is necessary to avoid deadlock. + inline void ReaderLock(); // Block until free or shared then acquire a share + inline void ReaderUnlock(); // Release a read share of this Mutex + inline void WriterLock() { Lock(); } // Acquire an exclusive lock + inline void WriterUnlock() { Unlock(); } // Release a lock from WriterLock() + + // TODO(hamaji): Do nothing, implement correctly. + inline void AssertHeld() {} + + private: + MutexType mutex_; + // We want to make sure that the compiler sets is_safe_ to true only + // when we tell it to, and never makes assumptions is_safe_ is + // always true. volatile is the most reliable way to do that. + volatile bool is_safe_; + + inline void SetIsSafe() { is_safe_ = true; } + + // Catch the error of writing Mutex when intending MutexLock. + Mutex(Mutex* /*ignored*/) {} + // Disallow "evil" constructors + Mutex(const Mutex&); + void operator=(const Mutex&); +}; + +// Now the implementation of Mutex for various systems +#if defined(CERES_NO_THREADS) + +// When we don't have threads, we can be either reading or writing, +// but not both. We can have lots of readers at once (in no-threads +// mode, that's most likely to happen in recursive function calls), +// but only one writer. We represent this by having mutex_ be -1 when +// writing and a number > 0 when reading (and 0 when no lock is held). +// +// In debug mode, we assert these invariants, while in non-debug mode +// we do nothing, for efficiency. That's why everything is in an +// assert. + +Mutex::Mutex() : mutex_(0) { } +Mutex::~Mutex() { assert(mutex_ == 0); } +void Mutex::Lock() { assert(--mutex_ == -1); } +void Mutex::Unlock() { assert(mutex_++ == -1); } +#ifdef CERES_GMUTEX_TRYLOCK +bool Mutex::TryLock() { if (mutex_) return false; Lock(); return true; } +#endif +void Mutex::ReaderLock() { assert(++mutex_ > 0); } +void Mutex::ReaderUnlock() { assert(mutex_-- > 0); } + +#elif defined(_WIN32) || defined(__CYGWIN32__) || defined(__CYGWIN64__) + +Mutex::Mutex() { InitializeCriticalSection(&mutex_); SetIsSafe(); } +Mutex::~Mutex() { DeleteCriticalSection(&mutex_); } +void Mutex::Lock() { if (is_safe_) EnterCriticalSection(&mutex_); } +void Mutex::Unlock() { if (is_safe_) LeaveCriticalSection(&mutex_); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + TryEnterCriticalSection(&mutex_) != 0 : true; } +#endif +void Mutex::ReaderLock() { Lock(); } // we don't have read-write locks +void Mutex::ReaderUnlock() { Unlock(); } + +#elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK) + +#define CERES_SAFE_PTHREAD(fncall) do { /* run fncall if is_safe_ is true */ \ + if (is_safe_ && fncall(&mutex_) != 0) abort(); \ +} while (0) + +Mutex::Mutex() { + SetIsSafe(); + if (is_safe_ && pthread_rwlock_init(&mutex_, NULL) != 0) abort(); +} +Mutex::~Mutex() { CERES_SAFE_PTHREAD(pthread_rwlock_destroy); } +void Mutex::Lock() { CERES_SAFE_PTHREAD(pthread_rwlock_wrlock); } +void Mutex::Unlock() { CERES_SAFE_PTHREAD(pthread_rwlock_unlock); } +#ifdef CERES_GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + pthread_rwlock_trywrlock(&mutex_) == 0 : + true; } +#endif +void Mutex::ReaderLock() { CERES_SAFE_PTHREAD(pthread_rwlock_rdlock); } +void Mutex::ReaderUnlock() { CERES_SAFE_PTHREAD(pthread_rwlock_unlock); } +#undef CERES_SAFE_PTHREAD + +#elif defined(CERES_HAVE_PTHREAD) + +#define CERES_SAFE_PTHREAD(fncall) do { /* run fncall if is_safe_ is true */ \ + if (is_safe_ && fncall(&mutex_) != 0) abort(); \ +} while (0) + +Mutex::Mutex() { + SetIsSafe(); + if (is_safe_ && pthread_mutex_init(&mutex_, NULL) != 0) abort(); +} +Mutex::~Mutex() { CERES_SAFE_PTHREAD(pthread_mutex_destroy); } +void Mutex::Lock() { CERES_SAFE_PTHREAD(pthread_mutex_lock); } +void Mutex::Unlock() { CERES_SAFE_PTHREAD(pthread_mutex_unlock); } +#ifdef CERES_GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + pthread_mutex_trylock(&mutex_) == 0 : true; } +#endif +void Mutex::ReaderLock() { Lock(); } +void Mutex::ReaderUnlock() { Unlock(); } +#undef CERES_SAFE_PTHREAD + +#endif + +// -------------------------------------------------------------------------- +// Some helper classes + +// Note: The weird "Ceres" prefix for the class is a workaround for having two +// similar mutex.h files included in the same translation unit. This is a +// problem because macros do not respect C++ namespaces, and as a result, this +// does not work well (e.g. inside Chrome). The offending macros are +// "MutexLock(x) COMPILE_ASSERT(false)". To work around this, "Ceres" is +// prefixed to the class names; this permits defining the classes. + +// CeresMutexLock(mu) acquires mu when constructed and releases it +// when destroyed. +class CeresMutexLock { + public: + explicit CeresMutexLock(Mutex *mu) : mu_(mu) { mu_->Lock(); } + ~CeresMutexLock() { mu_->Unlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + CeresMutexLock(const CeresMutexLock&); + void operator=(const CeresMutexLock&); +}; + +// CeresReaderMutexLock and CeresWriterMutexLock do the same, for rwlocks +class CeresReaderMutexLock { + public: + explicit CeresReaderMutexLock(Mutex *mu) : mu_(mu) { mu_->ReaderLock(); } + ~CeresReaderMutexLock() { mu_->ReaderUnlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + CeresReaderMutexLock(const CeresReaderMutexLock&); + void operator=(const CeresReaderMutexLock&); +}; + +class CeresWriterMutexLock { + public: + explicit CeresWriterMutexLock(Mutex *mu) : mu_(mu) { mu_->WriterLock(); } + ~CeresWriterMutexLock() { mu_->WriterUnlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + CeresWriterMutexLock(const CeresWriterMutexLock&); + void operator=(const CeresWriterMutexLock&); +}; + +// Catch bug where variable name is omitted, e.g. MutexLock (&mu); +#define CeresMutexLock(x) \ + COMPILE_ASSERT(0, ceres_mutex_lock_decl_missing_var_name) +#define CeresReaderMutexLock(x) \ + COMPILE_ASSERT(0, ceres_rmutex_lock_decl_missing_var_name) +#define CeresWriterMutexLock(x) \ + COMPILE_ASSERT(0, ceres_wmutex_lock_decl_missing_var_name) + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_MUTEX_H_ diff --git a/extern/ceres/internal/ceres/normal_prior.cc b/extern/ceres/internal/ceres/normal_prior.cc new file mode 100644 index 00000000000..b3666cd702f --- /dev/null +++ b/extern/ceres/internal/ceres/normal_prior.cc @@ -0,0 +1,66 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/normal_prior.h" + +#include <cstddef> +#include <vector> +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +NormalPrior::NormalPrior(const Matrix& A, const Vector& b) + : A_(A), b_(b) { + CHECK_GT(b_.rows(), 0); + CHECK_GT(A_.rows(), 0); + CHECK_EQ(b_.rows(), A.cols()); + set_num_residuals(A_.rows()); + mutable_parameter_block_sizes()->push_back(b_.rows()); +} + +bool NormalPrior::Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + ConstVectorRef p(parameters[0], parameter_block_sizes()[0]); + VectorRef r(residuals, num_residuals()); + // The following line should read + // r = A_ * (p - b_); + // The extra eval is to get around a bug in the eigen library. + r = A_ * (p - b_).eval(); + if ((jacobians != NULL) && (jacobians[0] != NULL)) { + MatrixRef(jacobians[0], num_residuals(), parameter_block_sizes()[0]) = A_; + } + return true; +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/parameter_block.h b/extern/ceres/internal/ceres/parameter_block.h new file mode 100644 index 00000000000..cb7140d9582 --- /dev/null +++ b/extern/ceres/internal/ceres/parameter_block.h @@ -0,0 +1,398 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_PARAMETER_BLOCK_H_ +#define CERES_INTERNAL_PARAMETER_BLOCK_H_ + +#include <algorithm> +#include <cstdlib> +#include <limits> +#include <string> +#include "ceres/array_utils.h" +#include "ceres/collections_port.h" +#include "ceres/integral_types.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/local_parameterization.h" +#include "ceres/stringprintf.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +class ProblemImpl; +class ResidualBlock; + +// The parameter block encodes the location of the user's original value, and +// also the "current state" of the parameter. The evaluator uses whatever is in +// the current state of the parameter when evaluating. This is inlined since the +// methods are performance sensitive. +// +// The class is not thread-safe, unless only const methods are called. The +// parameter block may also hold a pointer to a local parameterization; the +// parameter block does not take ownership of this pointer, so the user is +// responsible for the proper disposal of the local parameterization. +class ParameterBlock { + public: + // TODO(keir): Decide what data structure is best here. Should this be a set? + // Probably not, because sets are memory inefficient. However, if it's a + // vector, you can get into pathological linear performance when removing a + // residual block from a problem where all the residual blocks depend on one + // parameter; for example, shared focal length in a bundle adjustment + // problem. It might be worth making a custom structure that is just an array + // when it is small, but transitions to a hash set when it has more elements. + // + // For now, use a hash set. + typedef HashSet<ResidualBlock*> ResidualBlockSet; + + // Create a parameter block with the user state, size, and index specified. + // The size is the size of the parameter block and the index is the position + // of the parameter block inside a Program (if any). + ParameterBlock(double* user_state, int size, int index) { + Init(user_state, size, index, NULL); + } + + ParameterBlock(double* user_state, + int size, + int index, + LocalParameterization* local_parameterization) { + Init(user_state, size, index, local_parameterization); + } + + // The size of the parameter block. + int Size() const { return size_; } + + // Manipulate the parameter state. + bool SetState(const double* x) { + CHECK(x != NULL) + << "Tried to set the state of constant parameter " + << "with user location " << user_state_; + CHECK(!is_constant_) + << "Tried to set the state of constant parameter " + << "with user location " << user_state_; + + state_ = x; + return UpdateLocalParameterizationJacobian(); + } + + // Copy the current parameter state out to x. This is "GetState()" rather than + // simply "state()" since it is actively copying the data into the passed + // pointer. + void GetState(double *x) const { + if (x != state_) { + memcpy(x, state_, sizeof(*state_) * size_); + } + } + + // Direct pointers to the current state. + const double* state() const { return state_; } + const double* user_state() const { return user_state_; } + double* mutable_user_state() { return user_state_; } + LocalParameterization* local_parameterization() const { + return local_parameterization_; + } + LocalParameterization* mutable_local_parameterization() { + return local_parameterization_; + } + + // Set this parameter block to vary or not. + void SetConstant() { is_constant_ = true; } + void SetVarying() { is_constant_ = false; } + bool IsConstant() const { return is_constant_; } + + // This parameter block's index in an array. + int index() const { return index_; } + void set_index(int index) { index_ = index; } + + // This parameter offset inside a larger state vector. + int state_offset() const { return state_offset_; } + void set_state_offset(int state_offset) { state_offset_ = state_offset; } + + // This parameter offset inside a larger delta vector. + int delta_offset() const { return delta_offset_; } + void set_delta_offset(int delta_offset) { delta_offset_ = delta_offset; } + + // Methods relating to the parameter block's parameterization. + + // The local to global jacobian. Returns NULL if there is no local + // parameterization for this parameter block. The returned matrix is row-major + // and has Size() rows and LocalSize() columns. + const double* LocalParameterizationJacobian() const { + return local_parameterization_jacobian_.get(); + } + + int LocalSize() const { + return (local_parameterization_ == NULL) + ? size_ + : local_parameterization_->LocalSize(); + } + + // Set the parameterization. The parameterization can be set exactly once; + // multiple calls to set the parameterization to different values will crash. + // It is an error to pass NULL for the parameterization. The parameter block + // does not take ownership of the parameterization. + void SetParameterization(LocalParameterization* new_parameterization) { + CHECK(new_parameterization != NULL) << "NULL parameterization invalid."; + CHECK(new_parameterization->GlobalSize() == size_) + << "Invalid parameterization for parameter block. The parameter block " + << "has size " << size_ << " while the parameterization has a global " + << "size of " << new_parameterization->GlobalSize() << ". Did you " + << "accidentally use the wrong parameter block or parameterization?"; + if (new_parameterization != local_parameterization_) { + CHECK(local_parameterization_ == NULL) + << "Can't re-set the local parameterization; it leads to " + << "ambiguous ownership."; + local_parameterization_ = new_parameterization; + local_parameterization_jacobian_.reset( + new double[local_parameterization_->GlobalSize() * + local_parameterization_->LocalSize()]); + CHECK(UpdateLocalParameterizationJacobian()) + << "Local parameterization Jacobian computation failed for x: " + << ConstVectorRef(state_, Size()).transpose(); + } else { + // Ignore the case that the parameterizations match. + } + } + + void SetUpperBound(int index, double upper_bound) { + CHECK_LT(index, size_); + + if (upper_bounds_.get() == NULL) { + upper_bounds_.reset(new double[size_]); + std::fill(upper_bounds_.get(), + upper_bounds_.get() + size_, + std::numeric_limits<double>::max()); + } + + upper_bounds_[index] = upper_bound; + } + + void SetLowerBound(int index, double lower_bound) { + CHECK_LT(index, size_); + + if (lower_bounds_.get() == NULL) { + lower_bounds_.reset(new double[size_]); + std::fill(lower_bounds_.get(), + lower_bounds_.get() + size_, + -std::numeric_limits<double>::max()); + } + + lower_bounds_[index] = lower_bound; + } + + // Generalization of the addition operation. This is the same as + // LocalParameterization::Plus() followed by projection onto the + // hyper cube implied by the bounds constraints. + bool Plus(const double *x, const double* delta, double* x_plus_delta) { + if (local_parameterization_ != NULL) { + if (!local_parameterization_->Plus(x, delta, x_plus_delta)) { + return false; + } + } else { + VectorRef(x_plus_delta, size_) = ConstVectorRef(x, size_) + + ConstVectorRef(delta, size_); + } + + // Project onto the box constraints. + if (lower_bounds_.get() != NULL) { + for (int i = 0; i < size_; ++i) { + x_plus_delta[i] = std::max(x_plus_delta[i], lower_bounds_[i]); + } + } + + if (upper_bounds_.get() != NULL) { + for (int i = 0; i < size_; ++i) { + x_plus_delta[i] = std::min(x_plus_delta[i], upper_bounds_[i]); + } + } + + return true; + } + + std::string ToString() const { + return StringPrintf("{ this=%p, user_state=%p, state=%p, size=%d, " + "constant=%d, index=%d, state_offset=%d, " + "delta_offset=%d }", + this, + user_state_, + state_, + size_, + is_constant_, + index_, + state_offset_, + delta_offset_); + } + + void EnableResidualBlockDependencies() { + CHECK(residual_blocks_.get() == NULL) + << "Ceres bug: There is already a residual block collection " + << "for parameter block: " << ToString(); + residual_blocks_.reset(new ResidualBlockSet); + } + + void AddResidualBlock(ResidualBlock* residual_block) { + CHECK(residual_blocks_.get() != NULL) + << "Ceres bug: The residual block collection is null for parameter " + << "block: " << ToString(); + residual_blocks_->insert(residual_block); + } + + void RemoveResidualBlock(ResidualBlock* residual_block) { + CHECK(residual_blocks_.get() != NULL) + << "Ceres bug: The residual block collection is null for parameter " + << "block: " << ToString(); + CHECK(residual_blocks_->find(residual_block) != residual_blocks_->end()) + << "Ceres bug: Missing residual for parameter block: " << ToString(); + residual_blocks_->erase(residual_block); + } + + // This is only intended for iterating; perhaps this should only expose + // .begin() and .end(). + ResidualBlockSet* mutable_residual_blocks() { + return residual_blocks_.get(); + } + + double LowerBoundForParameter(int index) const { + if (lower_bounds_.get() == NULL) { + return -std::numeric_limits<double>::max(); + } else { + return lower_bounds_[index]; + } + } + + double UpperBoundForParameter(int index) const { + if (upper_bounds_.get() == NULL) { + return std::numeric_limits<double>::max(); + } else { + return upper_bounds_[index]; + } + } + + private: + void Init(double* user_state, + int size, + int index, + LocalParameterization* local_parameterization) { + user_state_ = user_state; + size_ = size; + index_ = index; + is_constant_ = false; + state_ = user_state_; + + local_parameterization_ = NULL; + if (local_parameterization != NULL) { + SetParameterization(local_parameterization); + } + + state_offset_ = -1; + delta_offset_ = -1; + } + + bool UpdateLocalParameterizationJacobian() { + if (local_parameterization_ == NULL) { + return true; + } + + // Update the local to global Jacobian. In some cases this is + // wasted effort; if this is a bottleneck, we will find a solution + // at that time. + + const int jacobian_size = Size() * LocalSize(); + InvalidateArray(jacobian_size, + local_parameterization_jacobian_.get()); + if (!local_parameterization_->ComputeJacobian( + state_, + local_parameterization_jacobian_.get())) { + LOG(WARNING) << "Local parameterization Jacobian computation failed" + "for x: " << ConstVectorRef(state_, Size()).transpose(); + return false; + } + + if (!IsArrayValid(jacobian_size, local_parameterization_jacobian_.get())) { + LOG(WARNING) << "Local parameterization Jacobian computation returned" + << "an invalid matrix for x: " + << ConstVectorRef(state_, Size()).transpose() + << "\n Jacobian matrix : " + << ConstMatrixRef(local_parameterization_jacobian_.get(), + Size(), + LocalSize()); + return false; + } + return true; + } + + double* user_state_; + int size_; + bool is_constant_; + LocalParameterization* local_parameterization_; + + // The "state" of the parameter. These fields are only needed while the + // solver is running. While at first glance using mutable is a bad idea, this + // ends up simplifying the internals of Ceres enough to justify the potential + // pitfalls of using "mutable." + mutable const double* state_; + mutable scoped_array<double> local_parameterization_jacobian_; + + // The index of the parameter. This is used by various other parts of Ceres to + // permit switching from a ParameterBlock* to an index in another array. + int32 index_; + + // The offset of this parameter block inside a larger state vector. + int32 state_offset_; + + // The offset of this parameter block inside a larger delta vector. + int32 delta_offset_; + + // If non-null, contains the residual blocks this parameter block is in. + scoped_ptr<ResidualBlockSet> residual_blocks_; + + // Upper and lower bounds for the parameter block. SetUpperBound + // and SetLowerBound lazily initialize the upper_bounds_ and + // lower_bounds_ arrays. If they are never called, then memory for + // these arrays is never allocated. Thus for problems where there + // are no bounds, or only one sided bounds we do not pay the cost of + // allocating memory for the inactive bounds constraints. + // + // Upon initialization these arrays are initialized to + // std::numeric_limits<double>::max() and + // -std::numeric_limits<double>::max() respectively which correspond + // to the parameter block being unconstrained. + scoped_array<double> upper_bounds_; + scoped_array<double> lower_bounds_; + + // Necessary so ProblemImpl can clean up the parameterizations. + friend class ProblemImpl; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PARAMETER_BLOCK_H_ diff --git a/extern/ceres/internal/ceres/parameter_block_ordering.cc b/extern/ceres/internal/ceres/parameter_block_ordering.cc new file mode 100644 index 00000000000..efba339977c --- /dev/null +++ b/extern/ceres/internal/ceres/parameter_block_ordering.cc @@ -0,0 +1,173 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/parameter_block_ordering.h" + +#include "ceres/graph.h" +#include "ceres/graph_algorithms.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/map_util.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/wall_time.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::map; +using std::set; +using std::vector; + +int ComputeStableSchurOrdering(const Program& program, + vector<ParameterBlock*>* ordering) { + CHECK_NOTNULL(ordering)->clear(); + EventLogger event_logger("ComputeStableSchurOrdering"); + scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); + event_logger.AddEvent("CreateHessianGraph"); + + 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]); + } + } + event_logger.AddEvent("Preordering"); + + int independent_set_size = StableIndependentSetOrdering(*graph, ordering); + event_logger.AddEvent("StableIndependentSet"); + + // 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); + } + } + event_logger.AddEvent("ConstantParameterBlocks"); + + return independent_set_size; +} + +int ComputeSchurOrdering(const Program& program, + vector<ParameterBlock*>* ordering) { + CHECK_NOTNULL(ordering)->clear(); + + scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); + int independent_set_size = IndependentSetOrdering(*graph, ordering); + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + + // Add the excluded blocks to back of the ordering vector. + 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; +} + +void ComputeRecursiveIndependentSetOrdering(const Program& program, + ParameterBlockOrdering* ordering) { + CHECK_NOTNULL(ordering)->Clear(); + const vector<ParameterBlock*> parameter_blocks = program.parameter_blocks(); + scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); + + int num_covered = 0; + int round = 0; + while (num_covered < parameter_blocks.size()) { + vector<ParameterBlock*> independent_set_ordering; + const int independent_set_size = + IndependentSetOrdering(*graph, &independent_set_ordering); + for (int i = 0; i < independent_set_size; ++i) { + ParameterBlock* parameter_block = independent_set_ordering[i]; + ordering->AddElementToGroup(parameter_block->mutable_user_state(), round); + graph->RemoveVertex(parameter_block); + } + num_covered += independent_set_size; + ++round; + } +} + +Graph<ParameterBlock*>* CreateHessianGraph(const Program& program) { + Graph<ParameterBlock*>* graph = CHECK_NOTNULL(new Graph<ParameterBlock*>); + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + if (!parameter_block->IsConstant()) { + graph->AddVertex(parameter_block); + } + } + + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + ParameterBlock* const* parameter_blocks = + residual_block->parameter_blocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + if (parameter_blocks[j]->IsConstant()) { + continue; + } + + for (int k = j + 1; k < num_parameter_blocks; ++k) { + if (parameter_blocks[k]->IsConstant()) { + continue; + } + + graph->AddEdge(parameter_blocks[j], parameter_blocks[k]); + } + } + } + + return graph; +} + +void OrderingToGroupSizes(const ParameterBlockOrdering* ordering, + vector<int>* group_sizes) { + CHECK_NOTNULL(group_sizes)->clear(); + if (ordering == NULL) { + return; + } + + const map<int, set<double*> >& group_to_elements = + ordering->group_to_elements(); + for (map<int, set<double*> >::const_iterator it = group_to_elements.begin(); + it != group_to_elements.end(); + ++it) { + group_sizes->push_back(it->second.size()); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/parameter_block_ordering.h b/extern/ceres/internal/ceres/parameter_block_ordering.h new file mode 100644 index 00000000000..f996929f6b3 --- /dev/null +++ b/extern/ceres/internal/ceres/parameter_block_ordering.h @@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_PARAMETER_BLOCK_ORDERING_H_ +#define CERES_INTERNAL_PARAMETER_BLOCK_ORDERING_H_ + +#include <vector> +#include "ceres/ordered_groups.h" +#include "ceres/graph.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class Program; +class ParameterBlock; + +// Uses an approximate independent set ordering to order the parameter +// blocks of a problem so that it is suitable for use with Schur +// complement based solvers. The output variable ordering contains an +// ordering of the parameter blocks and the return value is size of +// the independent set or the number of e_blocks (see +// schur_complement_solver.h for an explanation). Constant parameters +// are added to the end. +// +// The ordering vector has the structure +// +// ordering = [independent set, +// complement of the independent set, +// fixed blocks] +int ComputeSchurOrdering(const Program& program, + std::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, + std::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 +// the program. +void ComputeRecursiveIndependentSetOrdering(const Program& program, + ParameterBlockOrdering* ordering); + +// Builds a graph on the parameter blocks of a Problem, whose +// structure reflects the sparsity structure of the Hessian. Each +// vertex corresponds to a parameter block in the Problem except for +// parameter blocks that are marked constant. An edge connects two +// parameter blocks, if they co-occur in a residual block. +Graph<ParameterBlock*>* CreateHessianGraph(const Program& program); + +// Iterate over each of the groups in order of their priority and fill +// summary with their sizes. +void OrderingToGroupSizes(const ParameterBlockOrdering* ordering, + std::vector<int>* group_sizes); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PARAMETER_BLOCK_ORDERING_H_ diff --git a/extern/ceres/internal/ceres/partitioned_matrix_view.cc b/extern/ceres/internal/ceres/partitioned_matrix_view.cc new file mode 100644 index 00000000000..8054964e039 --- /dev/null +++ b/extern/ceres/internal/ceres/partitioned_matrix_view.cc @@ -0,0 +1,185 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_partitioned_matrix_view_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/linear_solver.h" +#include "ceres/partitioned_matrix_view.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +PartitionedMatrixViewBase* +PartitionedMatrixViewBase::Create(const LinearSolver::Options& options, + const BlockSparseMatrix& matrix) { +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 2)) { + return new PartitionedMatrixView<2, 2, 2>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 3)) { + return new PartitionedMatrixView<2, 2, 3>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 4)) { + return new PartitionedMatrixView<2, 2, 4>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == Eigen::Dynamic)) { + return new PartitionedMatrixView<2, 2, Eigen::Dynamic>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 3)) { + return new PartitionedMatrixView<2, 3, 3>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 4)) { + return new PartitionedMatrixView<2, 3, 4>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 6)) { + return new PartitionedMatrixView<2, 3, 6>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 9)) { + return new PartitionedMatrixView<2, 3, 9>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == Eigen::Dynamic)) { + return new PartitionedMatrixView<2, 3, Eigen::Dynamic>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new PartitionedMatrixView<2, 4, 3>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new PartitionedMatrixView<2, 4, 4>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 8)) { + return new PartitionedMatrixView<2, 4, 8>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 9)) { + return new PartitionedMatrixView<2, 4, 9>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == Eigen::Dynamic)) { + return new PartitionedMatrixView<2, 4, Eigen::Dynamic>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == Eigen::Dynamic) && + (options.f_block_size == Eigen::Dynamic)) { + return new PartitionedMatrixView<2, Eigen::Dynamic, Eigen::Dynamic>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 2)) { + return new PartitionedMatrixView<4, 4, 2>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new PartitionedMatrixView<4, 4, 3>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new PartitionedMatrixView<4, 4, 4>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == Eigen::Dynamic)) { + return new PartitionedMatrixView<4, 4, Eigen::Dynamic>( + matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == Eigen::Dynamic) && + (options.e_block_size == Eigen::Dynamic) && + (options.f_block_size == Eigen::Dynamic)) { + return new PartitionedMatrixView<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>( + matrix, options.elimination_groups[0]); + } + +#endif + VLOG(1) << "Template specializations not found for <" + << options.row_block_size << "," + << options.e_block_size << "," + << options.f_block_size << ">"; + return new PartitionedMatrixView<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>( + matrix, options.elimination_groups[0]); +}; + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/partitioned_matrix_view.h b/extern/ceres/internal/ceres/partitioned_matrix_view.h new file mode 100644 index 00000000000..6e75060a47e --- /dev/null +++ b/extern/ceres/internal/ceres/partitioned_matrix_view.h @@ -0,0 +1,152 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// For generalized bi-partite Jacobian matrices that arise in +// Structure from Motion related problems, it is sometimes useful to +// have access to the two parts of the matrix as linear operators +// themselves. This class provides that functionality. + +#ifndef CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ +#define CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ + +#include <algorithm> +#include <cstring> +#include <vector> + +#include "ceres/block_structure.h" +#include "ceres/internal/eigen.h" +#include "ceres/linear_solver.h" +#include "ceres/small_blas.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Given generalized bi-partite matrix A = [E F], with the same block +// structure as required by the Schur complement based solver, found +// in explicit_schur_complement_solver.h, provide access to the +// matrices E and F and their outer products E'E and F'F with +// themselves. +// +// Lack of BlockStructure object will result in a crash and if the +// block structure of the matrix does not satisfy the requirements of +// the Schur complement solver it will result in unpredictable and +// wrong output. +class PartitionedMatrixViewBase { + public: + virtual ~PartitionedMatrixViewBase() {} + + // y += E'x + virtual void LeftMultiplyE(const double* x, double* y) const = 0; + + // y += F'x + virtual void LeftMultiplyF(const double* x, double* y) const = 0; + + // y += Ex + virtual void RightMultiplyE(const double* x, double* y) const = 0; + + // y += Fx + virtual void RightMultiplyF(const double* x, double* y) const = 0; + + // Create and return the block diagonal of the matrix E'E. + virtual BlockSparseMatrix* CreateBlockDiagonalEtE() const = 0; + + // Create and return the block diagonal of the matrix F'F. Caller + // owns the result. + virtual BlockSparseMatrix* CreateBlockDiagonalFtF() const = 0; + + // Compute the block diagonal of the matrix E'E and store it in + // block_diagonal. The matrix block_diagonal is expected to have a + // BlockStructure (preferably created using + // CreateBlockDiagonalMatrixEtE) which is has the same structure as + // the block diagonal of E'E. + virtual void UpdateBlockDiagonalEtE( + BlockSparseMatrix* block_diagonal) const = 0; + + // Compute the block diagonal of the matrix F'F and store it in + // block_diagonal. The matrix block_diagonal is expected to have a + // BlockStructure (preferably created using + // CreateBlockDiagonalMatrixFtF) which is has the same structure as + // the block diagonal of F'F. + virtual void UpdateBlockDiagonalFtF( + BlockSparseMatrix* block_diagonal) const = 0; + + virtual int num_col_blocks_e() const = 0; + virtual int num_col_blocks_f() const = 0; + virtual int num_cols_e() const = 0; + virtual int num_cols_f() const = 0; + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; + + static PartitionedMatrixViewBase* Create(const LinearSolver::Options& options, + const BlockSparseMatrix& matrix); +}; + +template <int kRowBlockSize = Eigen::Dynamic, + int kEBlockSize = Eigen::Dynamic, + int kFBlockSize = Eigen::Dynamic > +class PartitionedMatrixView : public PartitionedMatrixViewBase { + public: + // matrix = [E F], where the matrix E contains the first + // num_col_blocks_a column blocks. + PartitionedMatrixView(const BlockSparseMatrix& matrix, int num_col_blocks_e); + + virtual ~PartitionedMatrixView(); + virtual void LeftMultiplyE(const double* x, double* y) const; + virtual void LeftMultiplyF(const double* x, double* y) const; + virtual void RightMultiplyE(const double* x, double* y) const; + virtual void RightMultiplyF(const double* x, double* y) const; + virtual BlockSparseMatrix* CreateBlockDiagonalEtE() const; + virtual BlockSparseMatrix* CreateBlockDiagonalFtF() const; + virtual void UpdateBlockDiagonalEtE(BlockSparseMatrix* block_diagonal) const; + virtual void UpdateBlockDiagonalFtF(BlockSparseMatrix* block_diagonal) const; + virtual int num_col_blocks_e() const { return num_col_blocks_e_; } + virtual int num_col_blocks_f() const { return num_col_blocks_f_; } + virtual int num_cols_e() const { return num_cols_e_; } + virtual int num_cols_f() const { return num_cols_f_; } + virtual int num_rows() const { return matrix_.num_rows(); } + virtual int num_cols() const { return matrix_.num_cols(); } + + private: + BlockSparseMatrix* CreateBlockDiagonalMatrixLayout(int start_col_block, + int end_col_block) const; + + const BlockSparseMatrix& matrix_; + int num_row_blocks_e_; + int num_col_blocks_e_; + int num_col_blocks_f_; + int num_cols_e_; + int num_cols_f_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ diff --git a/extern/ceres/internal/ceres/partitioned_matrix_view_impl.h b/extern/ceres/internal/ceres/partitioned_matrix_view_impl.h new file mode 100644 index 00000000000..86fb278fa27 --- /dev/null +++ b/extern/ceres/internal/ceres/partitioned_matrix_view_impl.h @@ -0,0 +1,380 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/partitioned_matrix_view.h" + +#include <algorithm> +#include <cstring> +#include <vector> +#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 { + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: +PartitionedMatrixView( + const BlockSparseMatrix& matrix, + int num_col_blocks_e) + : matrix_(matrix), + num_col_blocks_e_(num_col_blocks_e) { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + CHECK_NOTNULL(bs); + + num_col_blocks_f_ = bs->cols.size() - num_col_blocks_e_; + + // Compute the number of row blocks in E. The number of row blocks + // in E maybe less than the number of row blocks in the input matrix + // as some of the row blocks at the bottom may not have any + // e_blocks. For a definition of what an e_block is, please see + // explicit_schur_complement_solver.h + num_row_blocks_e_ = 0; + for (int r = 0; r < bs->rows.size(); ++r) { + const std::vector<Cell>& cells = bs->rows[r].cells; + if (cells[0].block_id < num_col_blocks_e_) { + ++num_row_blocks_e_; + } + } + + // Compute the number of columns in E and F. + num_cols_e_ = 0; + num_cols_f_ = 0; + + for (int c = 0; c < bs->cols.size(); ++c) { + const Block& block = bs->cols[c]; + if (c < num_col_blocks_e_) { + num_cols_e_ += block.size; + } else { + num_cols_f_ += block.size; + } + } + + CHECK_EQ(num_cols_e_ + num_cols_f_, matrix_.num_cols()); +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: +~PartitionedMatrixView() { +} + +// The next four methods don't seem to be particularly cache +// friendly. This is an artifact of how the BlockStructure of the +// input matrix is constructed. These methods will benefit from +// multithreading as well as improved data layout. + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: +RightMultiplyE(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over the first num_row_blocks_e_ row blocks, and multiply + // by the first cell in each row block. + const double* values = matrix_.values(); + for (int r = 0; r < num_row_blocks_e_; ++r) { + const Cell& cell = bs->rows[r].cells[0]; + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + const int col_block_id = cell.block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + MatrixVectorMultiply<kRowBlockSize, kEBlockSize, 1>( + values + cell.position, row_block_size, col_block_size, + x + col_block_pos, + y + row_block_pos); + } +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: +RightMultiplyF(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over row blocks, and if the row block is in E, then + // multiply by all the cells except the first one which is of type + // E. If the row block is not in E (i.e its in the bottom + // num_row_blocks - num_row_blocks_e row blocks), then all the cells + // are of type F and multiply by them all. + const double* values = matrix_.values(); + for (int r = 0; r < num_row_blocks_e_; ++r) { + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + const std::vector<Cell>& cells = bs->rows[r].cells; + for (int c = 1; c < cells.size(); ++c) { + 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<kRowBlockSize, kFBlockSize, 1>( + values + cells[c].position, row_block_size, col_block_size, + x + col_block_pos - num_cols_e_, + y + row_block_pos); + } + } + + for (int r = num_row_blocks_e_; 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 std::vector<Cell>& cells = bs->rows[r].cells; + for (int c = 0; c < cells.size(); ++c) { + 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>( + values + cells[c].position, row_block_size, col_block_size, + x + col_block_pos - num_cols_e_, + y + row_block_pos); + } + } +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: +LeftMultiplyE(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over the first num_row_blocks_e_ row blocks, and multiply + // by the first cell in each row block. + const double* values = matrix_.values(); + for (int r = 0; r < num_row_blocks_e_; ++r) { + const Cell& cell = bs->rows[r].cells[0]; + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + const int col_block_id = cell.block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>( + values + cell.position, row_block_size, col_block_size, + x + row_block_pos, + y + col_block_pos); + } +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: +LeftMultiplyF(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over row blocks, and if the row block is in E, then + // multiply by all the cells except the first one which is of type + // E. If the row block is not in E (i.e its in the bottom + // num_row_blocks - num_row_blocks_e row blocks), then all the cells + // are of type F and multiply by them all. + const double* values = matrix_.values(); + for (int r = 0; r < num_row_blocks_e_; ++r) { + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + const std::vector<Cell>& cells = bs->rows[r].cells; + for (int c = 1; c < cells.size(); ++c) { + 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<kRowBlockSize, kFBlockSize, 1>( + values + cells[c].position, row_block_size, col_block_size, + x + row_block_pos, + y + col_block_pos - num_cols_e_); + } + } + + for (int r = num_row_blocks_e_; 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 std::vector<Cell>& cells = bs->rows[r].cells; + for (int c = 0; c < cells.size(); ++c) { + 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>( + values + cells[c].position, row_block_size, col_block_size, + x + row_block_pos, + y + col_block_pos - num_cols_e_); + } + } +} + +// Given a range of columns blocks of a matrix m, compute the block +// structure of the block diagonal of the matrix m(:, +// start_col_block:end_col_block)'m(:, start_col_block:end_col_block) +// and return a BlockSparseMatrix with the this block structure. The +// caller owns the result. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +BlockSparseMatrix* +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: +CreateBlockDiagonalMatrixLayout(int start_col_block, int end_col_block) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + CompressedRowBlockStructure* block_diagonal_structure = + new CompressedRowBlockStructure; + + int block_position = 0; + int diagonal_cell_position = 0; + + // Iterate over the column blocks, creating a new diagonal block for + // each column block. + for (int c = start_col_block; c < end_col_block; ++c) { + const Block& block = bs->cols[c]; + block_diagonal_structure->cols.push_back(Block()); + Block& diagonal_block = block_diagonal_structure->cols.back(); + diagonal_block.size = block.size; + diagonal_block.position = block_position; + + block_diagonal_structure->rows.push_back(CompressedRow()); + CompressedRow& row = block_diagonal_structure->rows.back(); + row.block = diagonal_block; + + row.cells.push_back(Cell()); + Cell& cell = row.cells.back(); + cell.block_id = c - start_col_block; + cell.position = diagonal_cell_position; + + block_position += block.size; + diagonal_cell_position += block.size * block.size; + } + + // Build a BlockSparseMatrix with the just computed block + // structure. + return new BlockSparseMatrix(block_diagonal_structure); +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +BlockSparseMatrix* +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: +CreateBlockDiagonalEtE() const { + BlockSparseMatrix* block_diagonal = + CreateBlockDiagonalMatrixLayout(0, num_col_blocks_e_); + UpdateBlockDiagonalEtE(block_diagonal); + return block_diagonal; +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +BlockSparseMatrix* +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: +CreateBlockDiagonalFtF() const { + BlockSparseMatrix* block_diagonal = + CreateBlockDiagonalMatrixLayout( + num_col_blocks_e_, num_col_blocks_e_ + num_col_blocks_f_); + UpdateBlockDiagonalFtF(block_diagonal); + return block_diagonal; +} + +// Similar to the code in RightMultiplyE, except instead of the matrix +// vector multiply its an outer product. +// +// block_diagonal = block_diagonal(E'E) +// +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: +UpdateBlockDiagonalEtE( + BlockSparseMatrix* block_diagonal) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + + block_diagonal->SetZero(); + const double* values = matrix_.values(); + for (int r = 0; r < num_row_blocks_e_ ; ++r) { + const Cell& cell = bs->rows[r].cells[0]; + const int row_block_size = bs->rows[r].block.size; + const int block_id = cell.block_id; + const int col_block_size = bs->cols[block_id].size; + const int cell_position = + block_diagonal_structure->rows[block_id].cells[0].position; + + MatrixTransposeMatrixMultiply + <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>( + 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); + } +} + +// Similar to the code in RightMultiplyF, except instead of the matrix +// vector multiply its an outer product. +// +// block_diagonal = block_diagonal(F'F) +// +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: +UpdateBlockDiagonalFtF(BlockSparseMatrix* block_diagonal) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + + block_diagonal->SetZero(); + const double* values = matrix_.values(); + for (int r = 0; r < num_row_blocks_e_; ++r) { + const int row_block_size = bs->rows[r].block.size; + const std::vector<Cell>& cells = bs->rows[r].cells; + for (int c = 1; c < cells.size(); ++c) { + const int col_block_id = cells[c].block_id; + const int col_block_size = bs->cols[col_block_id].size; + const int diagonal_block_id = col_block_id - num_col_blocks_e_; + const int cell_position = + block_diagonal_structure->rows[diagonal_block_id].cells[0].position; + + MatrixTransposeMatrixMultiply + <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>( + 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); + } + } + + for (int r = num_row_blocks_e_; r < bs->rows.size(); ++r) { + const int row_block_size = bs->rows[r].block.size; + const std::vector<Cell>& cells = bs->rows[r].cells; + for (int c = 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; + const int diagonal_block_id = col_block_id - num_col_blocks_e_; + const int cell_position = + block_diagonal_structure->rows[diagonal_block_id].cells[0].position; + + MatrixTransposeMatrixMultiply + <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( + 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); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/polynomial.cc b/extern/ceres/internal/ceres/polynomial.cc new file mode 100644 index 00000000000..13bf8edeee6 --- /dev/null +++ b/extern/ceres/internal/ceres/polynomial.cc @@ -0,0 +1,398 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: moll.markus@arcor.de (Markus Moll) +// sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/polynomial.h" + +#include <cmath> +#include <cstddef> +#include <vector> + +#include "Eigen/Dense" +#include "ceres/internal/port.h" +#include "ceres/stringprintf.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::string; +using std::vector; + +namespace { + +// Balancing function as described by B. N. Parlett and C. Reinsch, +// "Balancing a Matrix for Calculation of Eigenvalues and Eigenvectors". +// In: Numerische Mathematik, Volume 13, Number 4 (1969), 293-304, +// Springer Berlin / Heidelberg. DOI: 10.1007/BF02165404 +void BalanceCompanionMatrix(Matrix* companion_matrix_ptr) { + CHECK_NOTNULL(companion_matrix_ptr); + Matrix& companion_matrix = *companion_matrix_ptr; + Matrix companion_matrix_offdiagonal = companion_matrix; + companion_matrix_offdiagonal.diagonal().setZero(); + + const int degree = companion_matrix.rows(); + + // gamma <= 1 controls how much a change in the scaling has to + // lower the 1-norm of the companion matrix to be accepted. + // + // gamma = 1 seems to lead to cycles (numerical issues?), so + // we set it slightly lower. + const double gamma = 0.9; + + // Greedily scale row/column pairs until there is no change. + bool scaling_has_changed; + do { + scaling_has_changed = false; + + for (int i = 0; i < degree; ++i) { + const double row_norm = companion_matrix_offdiagonal.row(i).lpNorm<1>(); + const double col_norm = companion_matrix_offdiagonal.col(i).lpNorm<1>(); + + // Decompose row_norm/col_norm into mantissa * 2^exponent, + // where 0.5 <= mantissa < 1. Discard mantissa (return value + // of frexp), as only the exponent is needed. + int exponent = 0; + std::frexp(row_norm / col_norm, &exponent); + exponent /= 2; + + if (exponent != 0) { + const double scaled_col_norm = std::ldexp(col_norm, exponent); + const double scaled_row_norm = std::ldexp(row_norm, -exponent); + if (scaled_col_norm + scaled_row_norm < gamma * (col_norm + row_norm)) { + // Accept the new scaling. (Multiplication by powers of 2 should not + // introduce rounding errors (ignoring non-normalized numbers and + // over- or underflow)) + scaling_has_changed = true; + companion_matrix_offdiagonal.row(i) *= std::ldexp(1.0, -exponent); + companion_matrix_offdiagonal.col(i) *= std::ldexp(1.0, exponent); + } + } + } + } while (scaling_has_changed); + + companion_matrix_offdiagonal.diagonal() = companion_matrix.diagonal(); + companion_matrix = companion_matrix_offdiagonal; + VLOG(3) << "Balanced companion matrix is\n" << companion_matrix; +} + +void BuildCompanionMatrix(const Vector& polynomial, + Matrix* companion_matrix_ptr) { + CHECK_NOTNULL(companion_matrix_ptr); + Matrix& companion_matrix = *companion_matrix_ptr; + + const int degree = polynomial.size() - 1; + + companion_matrix.resize(degree, degree); + companion_matrix.setZero(); + companion_matrix.diagonal(-1).setOnes(); + companion_matrix.col(degree - 1) = -polynomial.reverse().head(degree); +} + +// Remove leading terms with zero coefficients. +Vector RemoveLeadingZeros(const Vector& polynomial_in) { + int i = 0; + while (i < (polynomial_in.size() - 1) && polynomial_in(i) == 0.0) { + ++i; + } + return polynomial_in.tail(polynomial_in.size() - i); +} + +void FindLinearPolynomialRoots(const Vector& polynomial, + Vector* real, + Vector* imaginary) { + CHECK_EQ(polynomial.size(), 2); + if (real != NULL) { + real->resize(1); + (*real)(0) = -polynomial(1) / polynomial(0); + } + + if (imaginary != NULL) { + imaginary->setZero(1); + } +} + +void FindQuadraticPolynomialRoots(const Vector& polynomial, + Vector* real, + Vector* imaginary) { + CHECK_EQ(polynomial.size(), 3); + const double a = polynomial(0); + const double b = polynomial(1); + const double c = polynomial(2); + const double D = b * b - 4 * a * c; + const double sqrt_D = sqrt(fabs(D)); + if (real != NULL) { + real->setZero(2); + } + if (imaginary != NULL) { + imaginary->setZero(2); + } + + // Real roots. + if (D >= 0) { + if (real != NULL) { + // Stable quadratic roots according to BKP Horn. + // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf + if (b >= 0) { + (*real)(0) = (-b - sqrt_D) / (2.0 * a); + (*real)(1) = (2.0 * c) / (-b - sqrt_D); + } else { + (*real)(0) = (2.0 * c) / (-b + sqrt_D); + (*real)(1) = (-b + sqrt_D) / (2.0 * a); + } + } + return; + } + + // Use the normal quadratic formula for the complex case. + if (real != NULL) { + (*real)(0) = -b / (2.0 * a); + (*real)(1) = -b / (2.0 * a); + } + if (imaginary != NULL) { + (*imaginary)(0) = sqrt_D / (2.0 * a); + (*imaginary)(1) = -sqrt_D / (2.0 * a); + } +} +} // namespace + +bool FindPolynomialRoots(const Vector& polynomial_in, + Vector* real, + Vector* imaginary) { + if (polynomial_in.size() == 0) { + LOG(ERROR) << "Invalid polynomial of size 0 passed to FindPolynomialRoots"; + return false; + } + + Vector polynomial = RemoveLeadingZeros(polynomial_in); + const int degree = polynomial.size() - 1; + + VLOG(3) << "Input polynomial: " << polynomial_in.transpose(); + if (polynomial.size() != polynomial_in.size()) { + VLOG(3) << "Trimmed polynomial: " << polynomial.transpose(); + } + + // Is the polynomial constant? + if (degree == 0) { + LOG(WARNING) << "Trying to extract roots from a constant " + << "polynomial in FindPolynomialRoots"; + // We return true with no roots, not false, as if the polynomial is constant + // it is correct that there are no roots. It is not the case that they were + // there, but that we have failed to extract them. + return true; + } + + // Linear + if (degree == 1) { + FindLinearPolynomialRoots(polynomial, real, imaginary); + return true; + } + + // Quadratic + if (degree == 2) { + FindQuadraticPolynomialRoots(polynomial, real, imaginary); + return true; + } + + // The degree is now known to be at least 3. For cubic or higher + // roots we use the method of companion matrices. + + // Divide by leading term + const double leading_term = polynomial(0); + polynomial /= leading_term; + + // Build and balance the companion matrix to the polynomial. + Matrix companion_matrix(degree, degree); + BuildCompanionMatrix(polynomial, &companion_matrix); + BalanceCompanionMatrix(&companion_matrix); + + // Find its (complex) eigenvalues. + Eigen::EigenSolver<Matrix> solver(companion_matrix, false); + if (solver.info() != Eigen::Success) { + LOG(ERROR) << "Failed to extract eigenvalues from companion matrix."; + return false; + } + + // Output roots + if (real != NULL) { + *real = solver.eigenvalues().real(); + } else { + LOG(WARNING) << "NULL pointer passed as real argument to " + << "FindPolynomialRoots. Real parts of the roots will not " + << "be returned."; + } + if (imaginary != NULL) { + *imaginary = solver.eigenvalues().imag(); + } + return true; +} + +Vector DifferentiatePolynomial(const Vector& polynomial) { + const int degree = polynomial.rows() - 1; + CHECK_GE(degree, 0); + + // Degree zero polynomials are constants, and their derivative does + // not result in a smaller degree polynomial, just a degree zero + // polynomial with value zero. + if (degree == 0) { + return Eigen::VectorXd::Zero(1); + } + + Vector derivative(degree); + for (int i = 0; i < degree; ++i) { + derivative(i) = (degree - i) * polynomial(i); + } + + return derivative; +} + +void MinimizePolynomial(const Vector& polynomial, + const double x_min, + const double x_max, + double* optimal_x, + double* optimal_value) { + // Find the minimum of the polynomial at the two ends. + // + // We start by inspecting the middle of the interval. Technically + // this is not needed, but we do this to make this code as close to + // the minFunc package as possible. + *optimal_x = (x_min + x_max) / 2.0; + *optimal_value = EvaluatePolynomial(polynomial, *optimal_x); + + const double x_min_value = EvaluatePolynomial(polynomial, x_min); + if (x_min_value < *optimal_value) { + *optimal_value = x_min_value; + *optimal_x = x_min; + } + + const double x_max_value = EvaluatePolynomial(polynomial, x_max); + if (x_max_value < *optimal_value) { + *optimal_value = x_max_value; + *optimal_x = x_max; + } + + // If the polynomial is linear or constant, we are done. + if (polynomial.rows() <= 2) { + return; + } + + const Vector derivative = DifferentiatePolynomial(polynomial); + Vector roots_real; + if (!FindPolynomialRoots(derivative, &roots_real, NULL)) { + LOG(WARNING) << "Unable to find the critical points of " + << "the interpolating polynomial."; + return; + } + + // This is a bit of an overkill, as some of the roots may actually + // have a complex part, but its simpler to just check these values. + for (int i = 0; i < roots_real.rows(); ++i) { + const double root = roots_real(i); + if ((root < x_min) || (root > x_max)) { + continue; + } + + const double value = EvaluatePolynomial(polynomial, root); + if (value < *optimal_value) { + *optimal_value = value; + *optimal_x = root; + } + } +} + +string FunctionSample::ToDebugString() const { + return StringPrintf("[x: %.8e, value: %.8e, gradient: %.8e, " + "value_is_valid: %d, gradient_is_valid: %d]", + x, value, gradient, value_is_valid, gradient_is_valid); +} + +Vector FindInterpolatingPolynomial(const vector<FunctionSample>& samples) { + const int num_samples = samples.size(); + int num_constraints = 0; + for (int i = 0; i < num_samples; ++i) { + if (samples[i].value_is_valid) { + ++num_constraints; + } + if (samples[i].gradient_is_valid) { + ++num_constraints; + } + } + + const int degree = num_constraints - 1; + + Matrix lhs = Matrix::Zero(num_constraints, num_constraints); + Vector rhs = Vector::Zero(num_constraints); + + int row = 0; + for (int i = 0; i < num_samples; ++i) { + const FunctionSample& sample = samples[i]; + if (sample.value_is_valid) { + for (int j = 0; j <= degree; ++j) { + lhs(row, j) = pow(sample.x, degree - j); + } + rhs(row) = sample.value; + ++row; + } + + if (sample.gradient_is_valid) { + for (int j = 0; j < degree; ++j) { + lhs(row, j) = (degree - j) * pow(sample.x, degree - j - 1); + } + rhs(row) = sample.gradient; + ++row; + } + } + + return lhs.fullPivLu().solve(rhs); +} + +void MinimizeInterpolatingPolynomial(const vector<FunctionSample>& samples, + double x_min, + double x_max, + double* optimal_x, + double* optimal_value) { + const Vector polynomial = FindInterpolatingPolynomial(samples); + MinimizePolynomial(polynomial, x_min, x_max, optimal_x, optimal_value); + for (int i = 0; i < samples.size(); ++i) { + const FunctionSample& sample = samples[i]; + if ((sample.x < x_min) || (sample.x > x_max)) { + continue; + } + + const double value = EvaluatePolynomial(polynomial, sample.x); + if (value < *optimal_value) { + *optimal_x = sample.x; + *optimal_value = value; + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/polynomial.h b/extern/ceres/internal/ceres/polynomial.h new file mode 100644 index 00000000000..09a64c577f5 --- /dev/null +++ b/extern/ceres/internal/ceres/polynomial.h @@ -0,0 +1,136 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: moll.markus@arcor.de (Markus Moll) +// sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_POLYNOMIAL_SOLVER_H_ +#define CERES_INTERNAL_POLYNOMIAL_SOLVER_H_ + +#include <string> +#include <vector> +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +// All polynomials are assumed to be the form +// +// sum_{i=0}^N polynomial(i) x^{N-i}. +// +// and are given by a vector of coefficients of size N + 1. + +// Evaluate the polynomial at x using the Horner scheme. +inline double EvaluatePolynomial(const Vector& polynomial, double x) { + double v = 0.0; + for (int i = 0; i < polynomial.size(); ++i) { + v = v * x + polynomial(i); + } + return v; +} + +// Use the companion matrix eigenvalues to determine the roots of the +// polynomial. +// +// This function returns true on success, false otherwise. +// Failure indicates that the polynomial is invalid (of size 0) or +// that the eigenvalues of the companion matrix could not be computed. +// On failure, a more detailed message will be written to LOG(ERROR). +// If real is not NULL, the real parts of the roots will be returned in it. +// Likewise, if imaginary is not NULL, imaginary parts will be returned in it. +bool FindPolynomialRoots(const Vector& polynomial, + Vector* real, + Vector* imaginary); + +// Return the derivative of the given polynomial. It is assumed that +// the input polynomial is at least of degree zero. +Vector DifferentiatePolynomial(const Vector& polynomial); + +// Find the minimum value of the polynomial in the interval [x_min, +// x_max]. The minimum is obtained by computing all the roots of the +// derivative of the input polynomial. All real roots within the +// interval [x_min, x_max] are considered as well as the end points +// x_min and x_max. Since polynomials are differentiable functions, +// this ensures that the true minimum is found. +void MinimizePolynomial(const Vector& polynomial, + double x_min, + double x_max, + double* optimal_x, + double* optimal_value); + +// Structure for storing sample values of a function. +// +// Clients can use this struct to communicate the value of the +// function and or its gradient at a given point x. +struct FunctionSample { + FunctionSample() + : x(0.0), + value(0.0), + value_is_valid(false), + gradient(0.0), + gradient_is_valid(false) { + } + std::string ToDebugString() const; + + double x; + double value; // value = f(x) + bool value_is_valid; + double gradient; // gradient = f'(x) + bool gradient_is_valid; +}; + +// Given a set of function value and/or gradient samples, find a +// polynomial whose value and gradients are exactly equal to the ones +// in samples. +// +// Generally speaking, +// +// degree = # values + # gradients - 1 +// +// Of course its possible to sample a polynomial any number of times, +// in which case, generally speaking the spurious higher order +// coefficients will be zero. +Vector FindInterpolatingPolynomial(const std::vector<FunctionSample>& samples); + +// Interpolate the function described by samples with a polynomial, +// and minimize it on the interval [x_min, x_max]. Depending on the +// input samples, it is possible that the interpolation or the root +// finding algorithms may fail due to numerical difficulties. But the +// function is guaranteed to return its best guess of an answer, by +// considering the samples and the end points as possible solutions. +void MinimizeInterpolatingPolynomial(const std::vector<FunctionSample>& samples, + double x_min, + double x_max, + double* optimal_x, + double* optimal_value); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_POLYNOMIAL_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/preconditioner.cc b/extern/ceres/internal/ceres/preconditioner.cc new file mode 100644 index 00000000000..82621dae50c --- /dev/null +++ b/extern/ceres/internal/ceres/preconditioner.cc @@ -0,0 +1,73 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/preconditioner.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +Preconditioner::~Preconditioner() { +} + +PreconditionerType Preconditioner::PreconditionerForZeroEBlocks( + PreconditionerType preconditioner_type) { + if (preconditioner_type == SCHUR_JACOBI || + preconditioner_type == CLUSTER_JACOBI || + preconditioner_type == CLUSTER_TRIDIAGONAL) { + return JACOBI; + } + return preconditioner_type; +} + +SparseMatrixPreconditionerWrapper::SparseMatrixPreconditionerWrapper( + const SparseMatrix* matrix) + : matrix_(CHECK_NOTNULL(matrix)) { +} + +SparseMatrixPreconditionerWrapper::~SparseMatrixPreconditionerWrapper() { +} + +bool SparseMatrixPreconditionerWrapper::UpdateImpl(const SparseMatrix& A, + const double* D) { + return true; +} + +void SparseMatrixPreconditionerWrapper::RightMultiply(const double* x, + double* y) const { + matrix_->RightMultiply(x, y); +} + +int SparseMatrixPreconditionerWrapper::num_rows() const { + return matrix_->num_rows(); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/preconditioner.h b/extern/ceres/internal/ceres/preconditioner.h new file mode 100644 index 00000000000..a248eae060d --- /dev/null +++ b/extern/ceres/internal/ceres/preconditioner.h @@ -0,0 +1,177 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_PRECONDITIONER_H_ +#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" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; +class SparseMatrix; + +class Preconditioner : public LinearOperator { + public: + struct Options { + Options() + : type(JACOBI), + visibility_clustering_type(CANONICAL_VIEWS), + sparse_linear_algebra_library_type(SUITE_SPARSE), + num_threads(1), + row_block_size(Eigen::Dynamic), + e_block_size(Eigen::Dynamic), + f_block_size(Eigen::Dynamic) { + } + + PreconditionerType type; + VisibilityClusteringType visibility_clustering_type; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; + + // If possible, how many threads the preconditioner can use. + int num_threads; + + // Hints about the order in which the parameter blocks should be + // eliminated by the linear solver. + // + // For example if elimination_groups is a vector of size k, then + // the linear solver is informed that it should eliminate the + // parameter blocks 0 ... elimination_groups[0] - 1 first, and + // then elimination_groups[0] ... elimination_groups[1] - 1 and so + // on. Within each elimination group, the linear solver is free to + // choose how the parameter blocks are ordered. Different linear + // solvers have differing requirements on elimination_groups. + // + // The most common use is for Schur type solvers, where there + // should be at least two elimination groups and the first + // elimination group must form an independent set in the normal + // equations. The first elimination group corresponds to the + // num_eliminate_blocks in the Schur type solvers. + std::vector<int> elimination_groups; + + // If the block sizes in a BlockSparseMatrix are fixed, then in + // some cases the Schur complement based solvers can detect and + // specialize on them. + // + // It is expected that these parameters are set programmatically + // rather than manually. + // + // Please see schur_complement_solver.h and schur_eliminator.h for + // more details. + int row_block_size; + int e_block_size; + int f_block_size; + }; + + // If the optimization problem is such that there are no remaining + // e-blocks, ITERATIVE_SCHUR with a Schur type preconditioner cannot + // be used. This function returns JACOBI if a preconditioner for + // ITERATIVE_SCHUR is used. The input preconditioner_type is + // returned otherwise. + static PreconditionerType PreconditionerForZeroEBlocks( + PreconditionerType preconditioner_type); + + virtual ~Preconditioner(); + + // Update the numerical value of the preconditioner for the linear + // system: + // + // | A | x = |b| + // |diag(D)| |0| + // + // for some vector b. It is important that the matrix A have the + // same block structure as the one used to construct this object. + // + // D can be NULL, in which case its interpreted as a diagonal matrix + // of size zero. + 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 + // num_rows respectively. Update() must be called before + // RightMultiply can be called. + virtual void RightMultiply(const double* x, double* y) const = 0; + virtual void LeftMultiply(const double* x, double* y) const { + return RightMultiply(x, y); + } + + virtual int num_rows() const = 0; + virtual int num_cols() const { + return num_rows(); + } +}; + +// 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 SparseMatrixPreconditioner { + public: + // Wrapper does NOT take ownership of the matrix pointer. + explicit SparseMatrixPreconditionerWrapper(const SparseMatrix* matrix); + virtual ~SparseMatrixPreconditionerWrapper(); + + // Preconditioner interface + 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_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PRECONDITIONER_H_ diff --git a/extern/ceres/internal/ceres/preprocessor.cc b/extern/ceres/internal/ceres/preprocessor.cc new file mode 100644 index 00000000000..4aba6a39ce8 --- /dev/null +++ b/extern/ceres/internal/ceres/preprocessor.cc @@ -0,0 +1,113 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: sameragarwal@google.com (Sameer Agarwal) + +#include "ceres/callbacks.h" +#include "ceres/gradient_checking_cost_function.h" +#include "ceres/line_search_preprocessor.h" +#include "ceres/preprocessor.h" +#include "ceres/problem_impl.h" +#include "ceres/solver.h" +#include "ceres/trust_region_preprocessor.h" + +namespace ceres { +namespace internal { + +Preprocessor* Preprocessor::Create(MinimizerType minimizer_type) { + if (minimizer_type == TRUST_REGION) { + return new TrustRegionPreprocessor; + } + + if (minimizer_type == LINE_SEARCH) { + return new LineSearchPreprocessor; + } + + LOG(FATAL) << "Unknown minimizer_type: " << minimizer_type; + return NULL; +} + +Preprocessor::~Preprocessor() { +} + +void ChangeNumThreadsIfNeeded(Solver::Options* options) { +#ifndef CERES_USE_OPENMP + if (options->num_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads = 1 is supported. Switching " + << "to single threaded mode."; + options->num_threads = 1; + } + + // Only the Trust Region solver currently uses a linear solver. + if (options->minimizer_type == TRUST_REGION && + options->num_linear_solver_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_linear_solver_threads=1 is supported. Switching " + << "to single threaded mode."; + options->num_linear_solver_threads = 1; + } +#endif // CERES_USE_OPENMP +} + +void SetupCommonMinimizerOptions(PreprocessedProblem* pp) { + const Solver::Options& options = pp->options; + Program* program = pp->reduced_program.get(); + + // Assuming that the parameter blocks in the program have been + // reordered as needed, extract them into a contiguous vector. + pp->reduced_parameters.resize(program->NumParameters()); + double* reduced_parameters = pp->reduced_parameters.data(); + program->ParameterBlocksToStateVector(reduced_parameters); + + Minimizer::Options& minimizer_options = pp->minimizer_options; + minimizer_options = Minimizer::Options(options); + minimizer_options.evaluator = pp->evaluator; + + if (options.logging_type != SILENT) { + pp->logging_callback.reset( + new LoggingCallback(options.minimizer_type, + options.minimizer_progress_to_stdout)); + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + pp->logging_callback.get()); + } + + if (options.update_state_every_iteration) { + pp->state_updating_callback.reset( + new StateUpdatingCallback(program, reduced_parameters)); + // This must get pushed to the front of the callbacks so that it + // is run before any of the user callbacks. + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + pp->state_updating_callback.get()); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/preprocessor.h b/extern/ceres/internal/ceres/preprocessor.h new file mode 100644 index 00000000000..ff53d6f0d3f --- /dev/null +++ b/extern/ceres/internal/ceres/preprocessor.h @@ -0,0 +1,122 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: sameragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_PREPROCESSOR_H_ +#define CERES_INTERNAL_PREPROCESSOR_H_ + +#include <string> +#include <vector> + +#include "ceres/coordinate_descent_minimizer.h" +#include "ceres/evaluator.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/iteration_callback.h" +#include "ceres/linear_solver.h" +#include "ceres/minimizer.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/solver.h" + +namespace ceres { +namespace internal { + +struct PreprocessedProblem; + +// Given a Problem object and a Solver::Options object indicating the +// configuration of the solver, the job of the Preprocessor is to +// analyze the Problem and perform the setup needed to solve it using +// the desired Minimization algorithm. The setup involves removing +// redundancies in the input problem (inactive parameter and residual +// blocks), finding fill reducing orderings as needed, configuring and +// creating various objects needed by the Minimizer to solve the +// problem such as an evaluator, a linear solver etc. +// +// Each Minimizer (LineSearchMinimizer and TrustRegionMinimizer) comes +// with a corresponding Preprocessor (LineSearchPreprocessor and +// TrustRegionPreprocessor) that knows about its needs and performs +// the preprocessing needed. +// +// The output of the Preprocessor is stored in a PreprocessedProblem +// object. +class Preprocessor { + public: + // Factory. + static Preprocessor* Create(MinimizerType minimizer_type); + virtual ~Preprocessor(); + virtual bool Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* pp) = 0; +}; + +// A PreprocessedProblem is the result of running the Preprocessor on +// a Problem and Solver::Options object. +struct PreprocessedProblem { + PreprocessedProblem() + : fixed_cost(0.0) { + } + + std::string error; + Solver::Options options; + LinearSolver::Options linear_solver_options; + Evaluator::Options evaluator_options; + Minimizer::Options minimizer_options; + + ProblemImpl* problem; + scoped_ptr<ProblemImpl> gradient_checking_problem; + scoped_ptr<Program> reduced_program; + scoped_ptr<LinearSolver> linear_solver; + scoped_ptr<IterationCallback> logging_callback; + scoped_ptr<IterationCallback> state_updating_callback; + + shared_ptr<Evaluator> evaluator; + shared_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; + + std::vector<double*> removed_parameter_blocks; + Vector reduced_parameters; + double fixed_cost; +}; + +// Common functions used by various preprocessors. + +// If OpenMP support is not available and user has requested more than +// one thread, then set the *_num_threads options as needed to 1. +void ChangeNumThreadsIfNeeded(Solver::Options* options); + +// Extract the effective parameter vector from the preprocessed +// problem and setup bits of the Minimizer::Options object that are +// common to all Preprocessors. +void SetupCommonMinimizerOptions(PreprocessedProblem* pp); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PREPROCESSOR_H_ diff --git a/extern/ceres/internal/ceres/problem.cc b/extern/ceres/internal/ceres/problem.cc new file mode 100644 index 00000000000..03b7d6afa48 --- /dev/null +++ b/extern/ceres/internal/ceres/problem.cc @@ -0,0 +1,273 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#include "ceres/problem.h" + +#include <vector> +#include "ceres/crs_matrix.h" +#include "ceres/problem_impl.h" + +namespace ceres { + +using std::vector; + +Problem::Problem() : problem_impl_(new internal::ProblemImpl) {} +Problem::Problem(const Problem::Options& options) + : problem_impl_(new internal::ProblemImpl(options)) {} +Problem::~Problem() {} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + parameter_blocks); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3, x4); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3, x4, x5); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, + double* x6) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3, x4, x5, x6); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, + double* x6, double* x7) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3, x4, x5, x6, x7); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, + double* x6, double* x7, double* x8) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3, x4, x5, x6, x7, x8); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, + double* x6, double* x7, double* x8, double* x9) { + return problem_impl_->AddResidualBlock( + cost_function, + loss_function, + x0, x1, x2, x3, x4, x5, x6, x7, x8, x9); +} + +void Problem::AddParameterBlock(double* values, int size) { + problem_impl_->AddParameterBlock(values, size); +} + +void Problem::AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization) { + problem_impl_->AddParameterBlock(values, size, local_parameterization); +} + +void Problem::RemoveResidualBlock(ResidualBlockId residual_block) { + problem_impl_->RemoveResidualBlock(residual_block); +} + +void Problem::RemoveParameterBlock(double* values) { + problem_impl_->RemoveParameterBlock(values); +} + +void Problem::SetParameterBlockConstant(double* values) { + problem_impl_->SetParameterBlockConstant(values); +} + +void Problem::SetParameterBlockVariable(double* values) { + problem_impl_->SetParameterBlockVariable(values); +} + +void Problem::SetParameterization( + double* values, + LocalParameterization* local_parameterization) { + problem_impl_->SetParameterization(values, local_parameterization); +} + +const LocalParameterization* Problem::GetParameterization( + double* values) const { + return problem_impl_->GetParameterization(values); +} + +void Problem::SetParameterLowerBound(double* values, + int index, + double lower_bound) { + problem_impl_->SetParameterLowerBound(values, index, lower_bound); +} + +void Problem::SetParameterUpperBound(double* values, + int index, + double upper_bound) { + problem_impl_->SetParameterUpperBound(values, index, upper_bound); +} + +bool Problem::Evaluate(const EvaluateOptions& evaluate_options, + double* cost, + vector<double>* residuals, + vector<double>* gradient, + CRSMatrix* jacobian) { + return problem_impl_->Evaluate(evaluate_options, + cost, + residuals, + gradient, + jacobian); +} + +int Problem::NumParameterBlocks() const { + return problem_impl_->NumParameterBlocks(); +} + +int Problem::NumParameters() const { + return problem_impl_->NumParameters(); +} + +int Problem::NumResidualBlocks() const { + return problem_impl_->NumResidualBlocks(); +} + +int Problem::NumResiduals() const { + return problem_impl_->NumResiduals(); +} + +int Problem::ParameterBlockSize(const double* parameter_block) const { + return problem_impl_->ParameterBlockSize(parameter_block); +} + +int Problem::ParameterBlockLocalSize(const double* parameter_block) const { + return problem_impl_->ParameterBlockLocalSize(parameter_block); +} + +bool Problem::HasParameterBlock(const double* values) const { + return problem_impl_->HasParameterBlock(values); +} + +void Problem::GetParameterBlocks(vector<double*>* parameter_blocks) const { + problem_impl_->GetParameterBlocks(parameter_blocks); +} + +void Problem::GetResidualBlocks( + vector<ResidualBlockId>* residual_blocks) const { + problem_impl_->GetResidualBlocks(residual_blocks); +} + +void Problem::GetParameterBlocksForResidualBlock( + const ResidualBlockId residual_block, + vector<double*>* parameter_blocks) const { + problem_impl_->GetParameterBlocksForResidualBlock(residual_block, + parameter_blocks); +} + +const CostFunction* Problem::GetCostFunctionForResidualBlock( + const ResidualBlockId residual_block) const { + return problem_impl_->GetCostFunctionForResidualBlock(residual_block); +} + +const LossFunction* Problem::GetLossFunctionForResidualBlock( + const ResidualBlockId residual_block) const { + return problem_impl_->GetLossFunctionForResidualBlock(residual_block); +} + +void Problem::GetResidualBlocksForParameterBlock( + const double* values, + vector<ResidualBlockId>* residual_blocks) const { + problem_impl_->GetResidualBlocksForParameterBlock(values, + residual_blocks); +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/problem_impl.cc b/extern/ceres/internal/ceres/problem_impl.cc new file mode 100644 index 00000000000..8547d5d3f77 --- /dev/null +++ b/extern/ceres/internal/ceres/problem_impl.cc @@ -0,0 +1,945 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// mierle@gmail.com (Keir Mierle) + +#include "ceres/problem_impl.h" + +#include <algorithm> +#include <cstddef> +#include <iterator> +#include <set> +#include <string> +#include <utility> +#include <vector> +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/cost_function.h" +#include "ceres/crs_matrix.h" +#include "ceres/evaluator.h" +#include "ceres/loss_function.h" +#include "ceres/map_util.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/stl_util.h" +#include "ceres/stringprintf.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::map; +using std::string; +using std::vector; +typedef std::map<double*, internal::ParameterBlock*> ParameterMap; + +namespace { +// Returns true if two regions of memory, a and b, with sizes size_a and size_b +// respectively, overlap. +bool RegionsAlias(const double* a, int size_a, + const double* b, int size_b) { + return (a < b) ? b < (a + size_a) + : a < (b + size_b); +} + +void CheckForNoAliasing(double* existing_block, + int existing_block_size, + double* new_block, + int new_block_size) { + CHECK(!RegionsAlias(existing_block, existing_block_size, + new_block, new_block_size)) + << "Aliasing detected between existing parameter block at memory " + << "location " << existing_block + << " and has size " << existing_block_size << " with new parameter " + << "block that has memory address " << new_block << " and would have " + << "size " << new_block_size << "."; +} + +} // namespace + +ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, + int size) { + CHECK(values != NULL) << "Null pointer passed to AddParameterBlock " + << "for a parameter with size " << size; + + // Ignore the request if there is a block for the given pointer already. + ParameterMap::iterator it = parameter_block_map_.find(values); + if (it != parameter_block_map_.end()) { + if (!options_.disable_all_safety_checks) { + int existing_size = it->second->Size(); + CHECK(size == existing_size) + << "Tried adding a parameter block with the same double pointer, " + << values << ", twice, but with different block sizes. Original " + << "size was " << existing_size << " but new size is " + << size; + } + return it->second; + } + + if (!options_.disable_all_safety_checks) { + // Before adding the parameter block, also check that it doesn't alias any + // other parameter blocks. + if (!parameter_block_map_.empty()) { + ParameterMap::iterator lb = parameter_block_map_.lower_bound(values); + + // If lb is not the first block, check the previous block for aliasing. + if (lb != parameter_block_map_.begin()) { + ParameterMap::iterator previous = lb; + --previous; + CheckForNoAliasing(previous->first, + previous->second->Size(), + values, + size); + } + + // If lb is not off the end, check lb for aliasing. + if (lb != parameter_block_map_.end()) { + CheckForNoAliasing(lb->first, + lb->second->Size(), + values, + size); + } + } + } + + // Pass the index of the new parameter block as well to keep the index in + // sync with the position of the parameter in the program's parameter vector. + ParameterBlock* new_parameter_block = + new ParameterBlock(values, size, program_->parameter_blocks_.size()); + + // For dynamic problems, add the list of dependent residual blocks, which is + // empty to start. + if (options_.enable_fast_removal) { + new_parameter_block->EnableResidualBlockDependencies(); + } + parameter_block_map_[values] = new_parameter_block; + program_->parameter_blocks_.push_back(new_parameter_block); + return new_parameter_block; +} + +void ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) { + CHECK_NOTNULL(residual_block); + // Perform no check on the validity of residual_block, that is handled in + // the public method: RemoveResidualBlock(). + + // If needed, remove the parameter dependencies on this residual block. + if (options_.enable_fast_removal) { + const int num_parameter_blocks_for_residual = + residual_block->NumParameterBlocks(); + for (int i = 0; i < num_parameter_blocks_for_residual; ++i) { + residual_block->parameter_blocks()[i] + ->RemoveResidualBlock(residual_block); + } + + ResidualBlockSet::iterator it = residual_block_set_.find(residual_block); + residual_block_set_.erase(it); + } + DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block); +} + +// Deletes the residual block in question, assuming there are no other +// references to it inside the problem (e.g. by another parameter). Referenced +// cost and loss functions are tucked away for future deletion, since it is not +// possible to know whether other parts of the problem depend on them without +// doing a full scan. +void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) { + // The const casts here are legit, since ResidualBlock holds these + // pointers as const pointers but we have ownership of them and + // have the right to destroy them when the destructor is called. + if (options_.cost_function_ownership == TAKE_OWNERSHIP && + residual_block->cost_function() != NULL) { + cost_functions_to_delete_.push_back( + const_cast<CostFunction*>(residual_block->cost_function())); + } + if (options_.loss_function_ownership == TAKE_OWNERSHIP && + residual_block->loss_function() != NULL) { + loss_functions_to_delete_.push_back( + const_cast<LossFunction*>(residual_block->loss_function())); + } + delete residual_block; +} + +// Deletes the parameter block in question, assuming there are no other +// references to it inside the problem (e.g. by any residual blocks). +// Referenced parameterizations are tucked away for future deletion, since it +// is not possible to know whether other parts of the problem depend on them +// without doing a full scan. +void ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) { + if (options_.local_parameterization_ownership == TAKE_OWNERSHIP && + parameter_block->local_parameterization() != NULL) { + local_parameterizations_to_delete_.push_back( + parameter_block->mutable_local_parameterization()); + } + parameter_block_map_.erase(parameter_block->mutable_user_state()); + delete parameter_block; +} + +ProblemImpl::ProblemImpl() : program_(new internal::Program) {} +ProblemImpl::ProblemImpl(const Problem::Options& options) + : options_(options), + program_(new internal::Program) {} + +ProblemImpl::~ProblemImpl() { + // Collect the unique cost/loss functions and delete the residuals. + const int num_residual_blocks = program_->residual_blocks_.size(); + cost_functions_to_delete_.reserve(num_residual_blocks); + loss_functions_to_delete_.reserve(num_residual_blocks); + for (int i = 0; i < program_->residual_blocks_.size(); ++i) { + DeleteBlock(program_->residual_blocks_[i]); + } + + // Collect the unique parameterizations and delete the parameters. + for (int i = 0; i < program_->parameter_blocks_.size(); ++i) { + DeleteBlock(program_->parameter_blocks_[i]); + } + + // Delete the owned cost/loss functions and parameterizations. + STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(), + local_parameterizations_to_delete_.end()); + STLDeleteUniqueContainerPointers(cost_functions_to_delete_.begin(), + cost_functions_to_delete_.end()); + STLDeleteUniqueContainerPointers(loss_functions_to_delete_.begin(), + loss_functions_to_delete_.end()); +} + +ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks) { + CHECK_NOTNULL(cost_function); + CHECK_EQ(parameter_blocks.size(), + cost_function->parameter_block_sizes().size()); + + // Check the sizes match. + const vector<int32>& parameter_block_sizes = + cost_function->parameter_block_sizes(); + + if (!options_.disable_all_safety_checks) { + CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size()) + << "Number of blocks input is different than the number of blocks " + << "that the cost function expects."; + + // Check for duplicate parameter blocks. + vector<double*> sorted_parameter_blocks(parameter_blocks); + sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); + vector<double*>::const_iterator duplicate_items = + unique(sorted_parameter_blocks.begin(), + sorted_parameter_blocks.end()); + if (duplicate_items != sorted_parameter_blocks.end()) { + string blocks; + for (int i = 0; i < parameter_blocks.size(); ++i) { + blocks += StringPrintf(" %p ", parameter_blocks[i]); + } + + LOG(FATAL) << "Duplicate parameter blocks in a residual parameter " + << "are not allowed. Parameter block pointers: [" + << blocks << "]"; + } + } + + // Add parameter blocks and convert the double*'s to parameter blocks. + vector<ParameterBlock*> parameter_block_ptrs(parameter_blocks.size()); + for (int i = 0; i < parameter_blocks.size(); ++i) { + parameter_block_ptrs[i] = + InternalAddParameterBlock(parameter_blocks[i], + parameter_block_sizes[i]); + } + + if (!options_.disable_all_safety_checks) { + // Check that the block sizes match the block sizes expected by the + // cost_function. + for (int i = 0; i < parameter_block_ptrs.size(); ++i) { + CHECK_EQ(cost_function->parameter_block_sizes()[i], + parameter_block_ptrs[i]->Size()) + << "The cost function expects parameter block " << i + << " of size " << cost_function->parameter_block_sizes()[i] + << " but was given a block of size " + << parameter_block_ptrs[i]->Size(); + } + } + + ResidualBlock* new_residual_block = + new ResidualBlock(cost_function, + loss_function, + parameter_block_ptrs, + program_->residual_blocks_.size()); + + // Add dependencies on the residual to the parameter blocks. + if (options_.enable_fast_removal) { + for (int i = 0; i < parameter_blocks.size(); ++i) { + parameter_block_ptrs[i]->AddResidualBlock(new_residual_block); + } + } + + program_->residual_blocks_.push_back(new_residual_block); + + if (options_.enable_fast_removal) { + residual_block_set_.insert(new_residual_block); + } + + return new_residual_block; +} + +// Unfortunately, macros don't help much to reduce this code, and var args don't +// work because of the ambiguous case that there is no loss function. +ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + residual_parameters.push_back(x5); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, + double* x6) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + residual_parameters.push_back(x5); + residual_parameters.push_back(x6); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, + double* x6, double* x7) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + residual_parameters.push_back(x5); + residual_parameters.push_back(x6); + residual_parameters.push_back(x7); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, + double* x6, double* x7, double* x8) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + residual_parameters.push_back(x5); + residual_parameters.push_back(x6); + residual_parameters.push_back(x7); + residual_parameters.push_back(x8); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, + double* x6, double* x7, double* x8, double* x9) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + residual_parameters.push_back(x5); + residual_parameters.push_back(x6); + residual_parameters.push_back(x7); + residual_parameters.push_back(x8); + residual_parameters.push_back(x9); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +void ProblemImpl::AddParameterBlock(double* values, int size) { + InternalAddParameterBlock(values, size); +} + +void ProblemImpl::AddParameterBlock( + double* values, + int size, + LocalParameterization* local_parameterization) { + ParameterBlock* parameter_block = + InternalAddParameterBlock(values, size); + if (local_parameterization != NULL) { + parameter_block->SetParameterization(local_parameterization); + } +} + +// Delete a block from a vector of blocks, maintaining the indexing invariant. +// This is done in constant time by moving an element from the end of the +// vector over the element to remove, then popping the last element. It +// destroys the ordering in the interest of speed. +template<typename Block> +void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks, + Block* block_to_remove) { + CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove) + << "You found a Ceres bug! \n" + << "Block requested: " + << block_to_remove->ToString() << "\n" + << "Block present: " + << (*mutable_blocks)[block_to_remove->index()]->ToString(); + + // Prepare the to-be-moved block for the new, lower-in-index position by + // setting the index to the blocks final location. + Block* tmp = mutable_blocks->back(); + tmp->set_index(block_to_remove->index()); + + // Overwrite the to-be-deleted residual block with the one at the end. + (*mutable_blocks)[block_to_remove->index()] = tmp; + + DeleteBlock(block_to_remove); + + // The block is gone so shrink the vector of blocks accordingly. + mutable_blocks->pop_back(); +} + +void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) { + CHECK_NOTNULL(residual_block); + + // Verify that residual_block identifies a residual in the current problem. + const string residual_not_found_message = + StringPrintf("Residual block to remove: %p not found. This usually means " + "one of three things have happened:\n" + " 1) residual_block is uninitialised and points to a random " + "area in memory.\n" + " 2) residual_block represented a residual that was added to" + " the problem, but referred to a parameter block which has " + "since been removed, which removes all residuals which " + "depend on that parameter block, and was thus removed.\n" + " 3) residual_block referred to a residual that has already " + "been removed from the problem (by the user).", + residual_block); + if (options_.enable_fast_removal) { + CHECK(residual_block_set_.find(residual_block) != + residual_block_set_.end()) + << residual_not_found_message; + } else { + // Perform a full search over all current residuals. + CHECK(std::find(program_->residual_blocks().begin(), + program_->residual_blocks().end(), + residual_block) != program_->residual_blocks().end()) + << residual_not_found_message; + } + + InternalRemoveResidualBlock(residual_block); +} + +void ProblemImpl::RemoveParameterBlock(double* values) { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, values, NULL); + if (parameter_block == NULL) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "it can be removed."; + } + + if (options_.enable_fast_removal) { + // Copy the dependent residuals from the parameter block because the set of + // dependents will change after each call to RemoveResidualBlock(). + vector<ResidualBlock*> residual_blocks_to_remove( + parameter_block->mutable_residual_blocks()->begin(), + parameter_block->mutable_residual_blocks()->end()); + for (int i = 0; i < residual_blocks_to_remove.size(); ++i) { + InternalRemoveResidualBlock(residual_blocks_to_remove[i]); + } + } else { + // Scan all the residual blocks to remove ones that depend on the parameter + // block. Do the scan backwards since the vector changes while iterating. + const int num_residual_blocks = NumResidualBlocks(); + for (int i = num_residual_blocks - 1; i >= 0; --i) { + ResidualBlock* residual_block = + (*(program_->mutable_residual_blocks()))[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + if (residual_block->parameter_blocks()[j] == parameter_block) { + InternalRemoveResidualBlock(residual_block); + // The parameter blocks are guaranteed unique. + break; + } + } + } + } + DeleteBlockInVector(program_->mutable_parameter_blocks(), parameter_block); +} + +void ProblemImpl::SetParameterBlockConstant(double* values) { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, values, NULL); + if (parameter_block == NULL) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "it can be set constant."; + } + + parameter_block->SetConstant(); +} + +void ProblemImpl::SetParameterBlockVariable(double* values) { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, values, NULL); + if (parameter_block == NULL) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "it can be set varying."; + } + + parameter_block->SetVarying(); +} + +void ProblemImpl::SetParameterization( + double* values, + LocalParameterization* local_parameterization) { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, values, NULL); + if (parameter_block == NULL) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "you can set its local parameterization."; + } + + parameter_block->SetParameterization(local_parameterization); +} + +const LocalParameterization* ProblemImpl::GetParameterization( + double* values) const { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, values, NULL); + if (parameter_block == NULL) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "you can get its local parameterization."; + } + + return parameter_block->local_parameterization(); +} + +void ProblemImpl::SetParameterLowerBound(double* values, + int index, + double lower_bound) { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, values, NULL); + if (parameter_block == NULL) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "you can set a lower bound on one of its components."; + } + + parameter_block->SetLowerBound(index, lower_bound); +} + +void ProblemImpl::SetParameterUpperBound(double* values, + int index, + double upper_bound) { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, values, NULL); + if (parameter_block == NULL) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "you can set an upper bound on one of its components."; + } + parameter_block->SetUpperBound(index, upper_bound); +} + +bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, + double* cost, + vector<double>* residuals, + vector<double>* gradient, + CRSMatrix* jacobian) { + if (cost == NULL && + residuals == NULL && + gradient == NULL && + jacobian == NULL) { + LOG(INFO) << "Nothing to do."; + return true; + } + + // If the user supplied residual blocks, then use them, otherwise + // take the residual blocks from the underlying program. + Program program; + *program.mutable_residual_blocks() = + ((evaluate_options.residual_blocks.size() > 0) + ? evaluate_options.residual_blocks : program_->residual_blocks()); + + const vector<double*>& parameter_block_ptrs = + evaluate_options.parameter_blocks; + + vector<ParameterBlock*> variable_parameter_blocks; + vector<ParameterBlock*>& parameter_blocks = + *program.mutable_parameter_blocks(); + + if (parameter_block_ptrs.size() == 0) { + // The user did not provide any parameter blocks, so default to + // using all the parameter blocks in the order that they are in + // the underlying program object. + parameter_blocks = program_->parameter_blocks(); + } else { + // The user supplied a vector of parameter blocks. Using this list + // requires a number of steps. + + // 1. Convert double* into ParameterBlock* + parameter_blocks.resize(parameter_block_ptrs.size()); + for (int i = 0; i < parameter_block_ptrs.size(); ++i) { + parameter_blocks[i] = FindWithDefault(parameter_block_map_, + parameter_block_ptrs[i], + NULL); + if (parameter_blocks[i] == NULL) { + LOG(FATAL) << "No known parameter block for " + << "Problem::Evaluate::Options.parameter_blocks[" << i << "]" + << " = " << parameter_block_ptrs[i]; + } + } + + // 2. The user may have only supplied a subset of parameter + // blocks, so identify the ones that are not supplied by the user + // and are NOT constant. These parameter blocks are stored in + // variable_parameter_blocks. + // + // To ensure that the parameter blocks are not included in the + // columns of the jacobian, we need to make sure that they are + // constant during evaluation and then make them variable again + // after we are done. + vector<ParameterBlock*> all_parameter_blocks(program_->parameter_blocks()); + vector<ParameterBlock*> included_parameter_blocks( + program.parameter_blocks()); + + vector<ParameterBlock*> excluded_parameter_blocks; + sort(all_parameter_blocks.begin(), all_parameter_blocks.end()); + sort(included_parameter_blocks.begin(), included_parameter_blocks.end()); + set_difference(all_parameter_blocks.begin(), + all_parameter_blocks.end(), + included_parameter_blocks.begin(), + included_parameter_blocks.end(), + back_inserter(excluded_parameter_blocks)); + + variable_parameter_blocks.reserve(excluded_parameter_blocks.size()); + for (int i = 0; i < excluded_parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = excluded_parameter_blocks[i]; + if (!parameter_block->IsConstant()) { + variable_parameter_blocks.push_back(parameter_block); + parameter_block->SetConstant(); + } + } + } + + // Setup the Parameter indices and offsets before an evaluator can + // be constructed and used. + program.SetParameterOffsetsAndIndex(); + + Evaluator::Options evaluator_options; + + // Even though using SPARSE_NORMAL_CHOLESKY requires SuiteSparse or + // CXSparse, here it just being used for telling the evaluator to + // use a SparseRowCompressedMatrix for the jacobian. This is because + // the Evaluator decides the storage for the Jacobian based on the + // type of linear solver being used. + evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; +#ifndef CERES_USE_OPENMP + LOG_IF(WARNING, evaluate_options.num_threads > 1) + << "OpenMP support is not compiled into this binary; " + << "only evaluate_options.num_threads = 1 is supported. Switching " + << "to single threaded mode."; + evaluator_options.num_threads = 1; +#else + evaluator_options.num_threads = evaluate_options.num_threads; +#endif // CERES_USE_OPENMP + + string error; + scoped_ptr<Evaluator> evaluator( + Evaluator::Create(evaluator_options, &program, &error)); + if (evaluator.get() == NULL) { + LOG(ERROR) << "Unable to create an Evaluator object. " + << "Error: " << error + << "This is a Ceres bug; please contact the developers!"; + + // Make the parameter blocks that were temporarily marked + // constant, variable again. + for (int i = 0; i < variable_parameter_blocks.size(); ++i) { + variable_parameter_blocks[i]->SetVarying(); + } + + program_->SetParameterBlockStatePtrsToUserStatePtrs(); + program_->SetParameterOffsetsAndIndex(); + return false; + } + + if (residuals !=NULL) { + residuals->resize(evaluator->NumResiduals()); + } + + if (gradient != NULL) { + gradient->resize(evaluator->NumEffectiveParameters()); + } + + scoped_ptr<CompressedRowSparseMatrix> tmp_jacobian; + if (jacobian != NULL) { + tmp_jacobian.reset( + down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian())); + } + + // Point the state pointers to the user state pointers. This is + // needed so that we can extract a parameter vector which is then + // passed to Evaluator::Evaluate. + program.SetParameterBlockStatePtrsToUserStatePtrs(); + + // Copy the value of the parameter blocks into a vector, since the + // Evaluate::Evaluate method needs its input as such. The previous + // call to SetParameterBlockStatePtrsToUserStatePtrs ensures that + // these values are the ones corresponding to the actual state of + // the parameter blocks, rather than the temporary state pointer + // used for evaluation. + Vector parameters(program.NumParameters()); + program.ParameterBlocksToStateVector(parameters.data()); + + double tmp_cost = 0; + + Evaluator::EvaluateOptions evaluator_evaluate_options; + evaluator_evaluate_options.apply_loss_function = + evaluate_options.apply_loss_function; + bool status = evaluator->Evaluate(evaluator_evaluate_options, + parameters.data(), + &tmp_cost, + residuals != NULL ? &(*residuals)[0] : NULL, + gradient != NULL ? &(*gradient)[0] : NULL, + tmp_jacobian.get()); + + // Make the parameter blocks that were temporarily marked constant, + // variable again. + for (int i = 0; i < variable_parameter_blocks.size(); ++i) { + variable_parameter_blocks[i]->SetVarying(); + } + + if (status) { + if (cost != NULL) { + *cost = tmp_cost; + } + if (jacobian != NULL) { + tmp_jacobian->ToCRSMatrix(jacobian); + } + } + + program_->SetParameterBlockStatePtrsToUserStatePtrs(); + program_->SetParameterOffsetsAndIndex(); + return status; +} + +int ProblemImpl::NumParameterBlocks() const { + return program_->NumParameterBlocks(); +} + +int ProblemImpl::NumParameters() const { + return program_->NumParameters(); +} + +int ProblemImpl::NumResidualBlocks() const { + return program_->NumResidualBlocks(); +} + +int ProblemImpl::NumResiduals() const { + return program_->NumResiduals(); +} + +int ProblemImpl::ParameterBlockSize(const double* values) const { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL); + if (parameter_block == NULL) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "you can get its size."; + } + + return parameter_block->Size(); +} + +int ProblemImpl::ParameterBlockLocalSize(const double* values) const { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL); + if (parameter_block == NULL) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "you can get its local size."; + } + + return parameter_block->LocalSize(); +} + +bool ProblemImpl::HasParameterBlock(const double* parameter_block) const { + return (parameter_block_map_.find(const_cast<double*>(parameter_block)) != + parameter_block_map_.end()); +} + +void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const { + CHECK_NOTNULL(parameter_blocks); + parameter_blocks->resize(0); + for (ParameterMap::const_iterator it = parameter_block_map_.begin(); + it != parameter_block_map_.end(); + ++it) { + parameter_blocks->push_back(it->first); + } +} + +void ProblemImpl::GetResidualBlocks( + vector<ResidualBlockId>* residual_blocks) const { + CHECK_NOTNULL(residual_blocks); + *residual_blocks = program().residual_blocks(); +} + +void ProblemImpl::GetParameterBlocksForResidualBlock( + const ResidualBlockId residual_block, + vector<double*>* parameter_blocks) const { + int num_parameter_blocks = residual_block->NumParameterBlocks(); + CHECK_NOTNULL(parameter_blocks)->resize(num_parameter_blocks); + for (int i = 0; i < num_parameter_blocks; ++i) { + (*parameter_blocks)[i] = + residual_block->parameter_blocks()[i]->mutable_user_state(); + } +} + +const CostFunction* ProblemImpl::GetCostFunctionForResidualBlock( + const ResidualBlockId residual_block) const { + return residual_block->cost_function(); +} + +const LossFunction* ProblemImpl::GetLossFunctionForResidualBlock( + const ResidualBlockId residual_block) const { + return residual_block->loss_function(); +} + +void ProblemImpl::GetResidualBlocksForParameterBlock( + const double* values, + vector<ResidualBlockId>* residual_blocks) const { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL); + if (parameter_block == NULL) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "you can get the residual blocks that depend on it."; + } + + if (options_.enable_fast_removal) { + // In this case the residual blocks that depend on the parameter block are + // stored in the parameter block already, so just copy them out. + CHECK_NOTNULL(residual_blocks)->resize( + parameter_block->mutable_residual_blocks()->size()); + std::copy(parameter_block->mutable_residual_blocks()->begin(), + parameter_block->mutable_residual_blocks()->end(), + residual_blocks->begin()); + return; + } + + // Find residual blocks that depend on the parameter block. + CHECK_NOTNULL(residual_blocks)->clear(); + const int num_residual_blocks = NumResidualBlocks(); + for (int i = 0; i < num_residual_blocks; ++i) { + ResidualBlock* residual_block = + (*(program_->mutable_residual_blocks()))[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + if (residual_block->parameter_blocks()[j] == parameter_block) { + residual_blocks->push_back(residual_block); + // The parameter blocks are guaranteed unique. + break; + } + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/problem_impl.h b/extern/ceres/internal/ceres/problem_impl.h new file mode 100644 index 00000000000..f42bde6c793 --- /dev/null +++ b/extern/ceres/internal/ceres/problem_impl.h @@ -0,0 +1,226 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// This is the implementation of the public Problem API. The pointer to +// implementation (PIMPL) idiom makes it possible for Ceres internal code to +// refer to the private data members without needing to exposing it to the +// world. An alternative to PIMPL is to have a factory which returns instances +// of a virtual base class; while that approach would work, it requires clients +// to always put a Problem object into a scoped pointer; this needlessly muddies +// client code for little benefit. Therefore, the PIMPL comprise was chosen. + +#ifndef CERES_PUBLIC_PROBLEM_IMPL_H_ +#define CERES_PUBLIC_PROBLEM_IMPL_H_ + +#include <map> +#include <vector> + +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/collections_port.h" +#include "ceres/problem.h" +#include "ceres/types.h" + +namespace ceres { + +class CostFunction; +class LossFunction; +class LocalParameterization; +struct CRSMatrix; + +namespace internal { + +class Program; +class ResidualBlock; + +class ProblemImpl { + public: + typedef std::map<double*, ParameterBlock*> ParameterMap; + typedef HashSet<ResidualBlock*> ResidualBlockSet; + + ProblemImpl(); + explicit ProblemImpl(const Problem::Options& options); + + ~ProblemImpl(); + + // See the public problem.h file for description of these methods. + ResidualBlockId AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + const std::vector<double*>& parameter_blocks); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5, + double* x6); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5, + double* x6, double* x7); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5, + double* x6, double* x7, double* x8); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5, + double* x6, double* x7, double* x8, + double* x9); + void AddParameterBlock(double* values, int size); + void AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization); + + void RemoveResidualBlock(ResidualBlock* residual_block); + void RemoveParameterBlock(double* values); + + void SetParameterBlockConstant(double* values); + void SetParameterBlockVariable(double* values); + void SetParameterization(double* values, + LocalParameterization* local_parameterization); + const LocalParameterization* GetParameterization(double* values) const; + + void SetParameterLowerBound(double* values, int index, double lower_bound); + void SetParameterUpperBound(double* values, int index, double upper_bound); + + bool Evaluate(const Problem::EvaluateOptions& options, + double* cost, + std::vector<double>* residuals, + std::vector<double>* gradient, + CRSMatrix* jacobian); + + int NumParameterBlocks() const; + int NumParameters() const; + int NumResidualBlocks() const; + int NumResiduals() const; + + int ParameterBlockSize(const double* parameter_block) const; + int ParameterBlockLocalSize(const double* parameter_block) const; + + bool HasParameterBlock(const double* parameter_block) const; + + void GetParameterBlocks(std::vector<double*>* parameter_blocks) const; + void GetResidualBlocks(std::vector<ResidualBlockId>* residual_blocks) const; + + void GetParameterBlocksForResidualBlock( + const ResidualBlockId residual_block, + std::vector<double*>* parameter_blocks) const; + + const CostFunction* GetCostFunctionForResidualBlock( + const ResidualBlockId residual_block) const; + const LossFunction* GetLossFunctionForResidualBlock( + const ResidualBlockId residual_block) const; + + void GetResidualBlocksForParameterBlock( + const double* values, + std::vector<ResidualBlockId>* residual_blocks) const; + + const Program& program() const { return *program_; } + Program* mutable_program() { return program_.get(); } + + const ParameterMap& parameter_map() const { return parameter_block_map_; } + const ResidualBlockSet& residual_block_set() const { + CHECK(options_.enable_fast_removal) + << "Fast removal not enabled, residual_block_set is not maintained."; + return residual_block_set_; + } + + private: + ParameterBlock* InternalAddParameterBlock(double* values, int size); + void InternalRemoveResidualBlock(ResidualBlock* residual_block); + + bool InternalEvaluate(Program* program, + double* cost, + std::vector<double>* residuals, + std::vector<double>* gradient, + CRSMatrix* jacobian); + + // Delete the arguments in question. These differ from the Remove* functions + // in that they do not clean up references to the block to delete; they + // merely delete them. + template<typename Block> + void DeleteBlockInVector(std::vector<Block*>* mutable_blocks, + Block* block_to_remove); + void DeleteBlock(ResidualBlock* residual_block); + void DeleteBlock(ParameterBlock* parameter_block); + + const Problem::Options options_; + + // The mapping from user pointers to parameter blocks. + std::map<double*, ParameterBlock*> parameter_block_map_; + + // Iff enable_fast_removal is enabled, contains the current residual blocks. + ResidualBlockSet residual_block_set_; + + // The actual parameter and residual blocks. + internal::scoped_ptr<internal::Program> program_; + + // When removing residual and parameter blocks, cost/loss functions and + // parameterizations have ambiguous ownership. Instead of scanning the entire + // problem to see if the cost/loss/parameterization is shared with other + // residual or parameter blocks, buffer them until destruction. + // + // TODO(keir): See if it makes sense to use sets instead. + std::vector<CostFunction*> cost_functions_to_delete_; + std::vector<LossFunction*> loss_functions_to_delete_; + std::vector<LocalParameterization*> local_parameterizations_to_delete_; + + CERES_DISALLOW_COPY_AND_ASSIGN(ProblemImpl); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_PROBLEM_IMPL_H_ diff --git a/extern/ceres/internal/ceres/program.cc b/extern/ceres/internal/ceres/program.cc new file mode 100644 index 00000000000..8e97f072113 --- /dev/null +++ b/extern/ceres/internal/ceres/program.cc @@ -0,0 +1,524 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/program.h" + +#include <map> +#include <vector> +#include "ceres/array_utils.h" +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/cost_function.h" +#include "ceres/evaluator.h" +#include "ceres/internal/port.h" +#include "ceres/local_parameterization.h" +#include "ceres/loss_function.h" +#include "ceres/map_util.h" +#include "ceres/parameter_block.h" +#include "ceres/problem.h" +#include "ceres/residual_block.h" +#include "ceres/stl_util.h" +#include "ceres/triplet_sparse_matrix.h" + +namespace ceres { +namespace internal { + +using std::max; +using std::set; +using std::string; +using std::vector; + +Program::Program() {} + +Program::Program(const Program& program) + : parameter_blocks_(program.parameter_blocks_), + residual_blocks_(program.residual_blocks_) { +} + +const vector<ParameterBlock*>& Program::parameter_blocks() const { + return parameter_blocks_; +} + +const vector<ResidualBlock*>& Program::residual_blocks() const { + return residual_blocks_; +} + +vector<ParameterBlock*>* Program::mutable_parameter_blocks() { + return ¶meter_blocks_; +} + +vector<ResidualBlock*>* Program::mutable_residual_blocks() { + return &residual_blocks_; +} + +bool Program::StateVectorToParameterBlocks(const double *state) { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + if (!parameter_blocks_[i]->IsConstant() && + !parameter_blocks_[i]->SetState(state)) { + return false; + } + state += parameter_blocks_[i]->Size(); + } + return true; +} + +void Program::ParameterBlocksToStateVector(double *state) const { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->GetState(state); + state += parameter_blocks_[i]->Size(); + } +} + +void Program::CopyParameterBlockStateToUserState() { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->GetState(parameter_blocks_[i]->mutable_user_state()); + } +} + +bool Program::SetParameterBlockStatePtrsToUserStatePtrs() { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + if (!parameter_blocks_[i]->IsConstant() && + !parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) { + return false; + } + } + return true; +} + +bool Program::Plus(const double* state, + const double* delta, + double* state_plus_delta) const { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) { + return false; + } + state += parameter_blocks_[i]->Size(); + delta += parameter_blocks_[i]->LocalSize(); + state_plus_delta += parameter_blocks_[i]->Size(); + } + return true; +} + +void Program::SetParameterOffsetsAndIndex() { + // Set positions for all parameters appearing as arguments to residuals to one + // past the end of the parameter block array. + for (int i = 0; i < residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = residual_blocks_[i]; + for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { + residual_block->parameter_blocks()[j]->set_index(-1); + } + } + // For parameters that appear in the program, set their position and offset. + int state_offset = 0; + int delta_offset = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->set_index(i); + parameter_blocks_[i]->set_state_offset(state_offset); + parameter_blocks_[i]->set_delta_offset(delta_offset); + state_offset += parameter_blocks_[i]->Size(); + delta_offset += parameter_blocks_[i]->LocalSize(); + } +} + +bool Program::IsValid() const { + for (int i = 0; i < residual_blocks_.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks_[i]; + if (residual_block->index() != i) { + LOG(WARNING) << "Residual block: " << i + << " has incorrect index: " << residual_block->index(); + return false; + } + } + + int state_offset = 0; + int delta_offset = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + if (parameter_block->index() != i || + parameter_block->state_offset() != state_offset || + parameter_block->delta_offset() != delta_offset) { + LOG(WARNING) << "Parameter block: " << i + << "has incorrect indexing information: " + << parameter_block->ToString(); + return false; + } + + state_offset += parameter_blocks_[i]->Size(); + delta_offset += parameter_blocks_[i]->LocalSize(); + } + + return true; +} + +bool Program::ParameterBlocksAreFinite(string* message) const { + CHECK_NOTNULL(message); + for (int i = 0; i < parameter_blocks_.size(); ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + const double* array = parameter_block->user_state(); + const int size = parameter_block->Size(); + const int invalid_index = FindInvalidValue(size, array); + if (invalid_index != size) { + *message = StringPrintf( + "ParameterBlock: %p with size %d has at least one invalid value.\n" + "First invalid value is at index: %d.\n" + "Parameter block values: ", + array, size, invalid_index); + AppendArrayToString(size, array, message); + return false; + } + } + return true; +} + +bool Program::IsBoundsConstrained() const { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + if (parameter_block->IsConstant()) { + continue; + } + const int size = parameter_block->Size(); + for (int j = 0; j < size; ++j) { + const double lower_bound = parameter_block->LowerBoundForParameter(j); + const double upper_bound = parameter_block->UpperBoundForParameter(j); + if (lower_bound > -std::numeric_limits<double>::max() || + upper_bound < std::numeric_limits<double>::max()) { + return true; + } + } + } + return false; +} + +bool Program::IsFeasible(string* message) const { + CHECK_NOTNULL(message); + for (int i = 0; i < parameter_blocks_.size(); ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + const double* parameters = parameter_block->user_state(); + const int size = parameter_block->Size(); + if (parameter_block->IsConstant()) { + // Constant parameter blocks must start in the feasible region + // to ultimately produce a feasible solution, since Ceres cannot + // change them. + for (int j = 0; j < size; ++j) { + const double lower_bound = parameter_block->LowerBoundForParameter(j); + const double upper_bound = parameter_block->UpperBoundForParameter(j); + if (parameters[j] < lower_bound || parameters[j] > upper_bound) { + *message = StringPrintf( + "ParameterBlock: %p with size %d has at least one infeasible " + "value." + "\nFirst infeasible value is at index: %d." + "\nLower bound: %e, value: %e, upper bound: %e" + "\nParameter block values: ", + parameters, size, j, lower_bound, parameters[j], upper_bound); + AppendArrayToString(size, parameters, message); + return false; + } + } + } else { + // Variable parameter blocks must have non-empty feasible + // regions, otherwise there is no way to produce a feasible + // solution. + for (int j = 0; j < size; ++j) { + const double lower_bound = parameter_block->LowerBoundForParameter(j); + const double upper_bound = parameter_block->UpperBoundForParameter(j); + if (lower_bound >= upper_bound) { + *message = StringPrintf( + "ParameterBlock: %p with size %d has at least one infeasible " + "bound." + "\nFirst infeasible bound is at index: %d." + "\nLower bound: %e, upper bound: %e" + "\nParameter block values: ", + parameters, size, j, lower_bound, upper_bound); + AppendArrayToString(size, parameters, message); + return false; + } + } + } + } + + return true; +} + +Program* Program::CreateReducedProgram( + vector<double*>* removed_parameter_blocks, + double* fixed_cost, + string* error) const { + CHECK_NOTNULL(removed_parameter_blocks); + CHECK_NOTNULL(fixed_cost); + CHECK_NOTNULL(error); + + scoped_ptr<Program> reduced_program(new Program(*this)); + if (!reduced_program->RemoveFixedBlocks(removed_parameter_blocks, + fixed_cost, + error)) { + return NULL; + } + + reduced_program->SetParameterOffsetsAndIndex(); + return reduced_program.release(); +} + +bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks, + double* fixed_cost, + string* error) { + CHECK_NOTNULL(removed_parameter_blocks); + CHECK_NOTNULL(fixed_cost); + CHECK_NOTNULL(error); + + scoped_array<double> residual_block_evaluate_scratch; + residual_block_evaluate_scratch.reset( + new double[MaxScratchDoublesNeededForEvaluate()]); + *fixed_cost = 0.0; + + // Mark all the parameters as unused. Abuse the index member of the + // parameter blocks for the marking. + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->set_index(-1); + } + + // Filter out residual that have all-constant parameters, and mark + // all the parameter blocks that appear in residuals. + int num_active_residual_blocks = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = residual_blocks_[i]; + int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Determine if the residual block is fixed, and also mark varying + // parameters that appear in the residual block. + bool all_constant = true; + for (int k = 0; k < num_parameter_blocks; k++) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[k]; + if (!parameter_block->IsConstant()) { + all_constant = false; + parameter_block->set_index(1); + } + } + + if (!all_constant) { + residual_blocks_[num_active_residual_blocks++] = residual_block; + continue; + } + + // The residual is constant and will be removed, so its cost is + // added to the variable fixed_cost. + double cost = 0.0; + if (!residual_block->Evaluate(true, + &cost, + NULL, + NULL, + residual_block_evaluate_scratch.get())) { + *error = StringPrintf("Evaluation of the residual %d failed during " + "removal of fixed residual blocks.", i); + return false; + } + *fixed_cost += cost; + } + residual_blocks_.resize(num_active_residual_blocks); + + // Filter out unused or fixed parameter blocks. + int num_active_parameter_blocks = 0; + removed_parameter_blocks->clear(); + for (int i = 0; i < parameter_blocks_.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks_[i]; + if (parameter_block->index() == -1) { + removed_parameter_blocks->push_back( + parameter_block->mutable_user_state()); + } else { + parameter_blocks_[num_active_parameter_blocks++] = parameter_block; + } + } + parameter_blocks_.resize(num_active_parameter_blocks); + + if (!(((NumResidualBlocks() == 0) && + (NumParameterBlocks() == 0)) || + ((NumResidualBlocks() != 0) && + (NumParameterBlocks() != 0)))) { + *error = "Congratulations, you found a bug in Ceres. Please report it."; + return false; + } + + return true; +} + +bool Program::IsParameterBlockSetIndependent( + const set<double*>& independent_set) const { + // Loop over each residual block and ensure that no two parameter + // blocks in the same residual block are part of + // parameter_block_ptrs as that would violate the assumption that it + // is an independent set in the Hessian matrix. + for (vector<ResidualBlock*>::const_iterator it = residual_blocks_.begin(); + it != residual_blocks_.end(); + ++it) { + ParameterBlock* const* parameter_blocks = (*it)->parameter_blocks(); + const int num_parameter_blocks = (*it)->NumParameterBlocks(); + int count = 0; + for (int i = 0; i < num_parameter_blocks; ++i) { + count += independent_set.count( + parameter_blocks[i]->mutable_user_state()); + } + if (count > 1) { + return false; + } + } + return true; +} + +TripletSparseMatrix* Program::CreateJacobianBlockSparsityTranspose() const { + // Matrix to store the block sparsity structure of the Jacobian. + TripletSparseMatrix* tsm = + new TripletSparseMatrix(NumParameterBlocks(), + NumResidualBlocks(), + 10 * NumResidualBlocks()); + int num_nonzeros = 0; + int* rows = tsm->mutable_rows(); + int* cols = tsm->mutable_cols(); + double* values = tsm->mutable_values(); + + for (int c = 0; c < residual_blocks_.size(); ++c) { + const ResidualBlock* residual_block = residual_blocks_[c]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + ParameterBlock* const* parameter_blocks = + residual_block->parameter_blocks(); + + for (int j = 0; j < num_parameter_blocks; ++j) { + if (parameter_blocks[j]->IsConstant()) { + continue; + } + + // 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(); + values = tsm->mutable_values(); + } + + const int r = parameter_blocks[j]->index(); + rows[num_nonzeros] = r; + cols[num_nonzeros] = c; + values[num_nonzeros] = 1.0; + ++num_nonzeros; + } + } + + tsm->set_num_nonzeros(num_nonzeros); + return tsm; +} + +int Program::NumResidualBlocks() const { + return residual_blocks_.size(); +} + +int Program::NumParameterBlocks() const { + return parameter_blocks_.size(); +} + +int Program::NumResiduals() const { + int num_residuals = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + num_residuals += residual_blocks_[i]->NumResiduals(); + } + return num_residuals; +} + +int Program::NumParameters() const { + int num_parameters = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + num_parameters += parameter_blocks_[i]->Size(); + } + return num_parameters; +} + +int Program::NumEffectiveParameters() const { + int num_parameters = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + num_parameters += parameter_blocks_[i]->LocalSize(); + } + return num_parameters; +} + +int Program::MaxScratchDoublesNeededForEvaluate() const { + // Compute the scratch space needed for evaluate. + int max_scratch_bytes_for_evaluate = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + max_scratch_bytes_for_evaluate = + max(max_scratch_bytes_for_evaluate, + residual_blocks_[i]->NumScratchDoublesForEvaluate()); + } + return max_scratch_bytes_for_evaluate; +} + +int Program::MaxDerivativesPerResidualBlock() const { + int max_derivatives = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + int derivatives = 0; + ResidualBlock* residual_block = residual_blocks_[i]; + int num_parameters = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameters; ++j) { + derivatives += residual_block->NumResiduals() * + residual_block->parameter_blocks()[j]->LocalSize(); + } + max_derivatives = max(max_derivatives, derivatives); + } + return max_derivatives; +} + +int Program::MaxParametersPerResidualBlock() const { + int max_parameters = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + max_parameters = max(max_parameters, + residual_blocks_[i]->NumParameterBlocks()); + } + return max_parameters; +} + +int Program::MaxResidualsPerResidualBlock() const { + int max_residuals = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + max_residuals = max(max_residuals, residual_blocks_[i]->NumResiduals()); + } + return max_residuals; +} + +string Program::ToString() const { + string ret = "Program dump\n"; + ret += StringPrintf("Number of parameter blocks: %d\n", NumParameterBlocks()); + ret += StringPrintf("Number of parameters: %d\n", NumParameters()); + ret += "Parameters:\n"; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + ret += StringPrintf("%d: %s\n", + i, parameter_blocks_[i]->ToString().c_str()); + } + return ret; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/program.h b/extern/ceres/internal/ceres/program.h new file mode 100644 index 00000000000..38c958fe34a --- /dev/null +++ b/extern/ceres/internal/ceres/program.h @@ -0,0 +1,192 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_PROGRAM_H_ +#define CERES_INTERNAL_PROGRAM_H_ + +#include <set> +#include <string> +#include <vector> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class ParameterBlock; +class ProblemImpl; +class ResidualBlock; +class TripletSparseMatrix; + +// A nonlinear least squares optimization problem. This is different from the +// similarly-named "Problem" object, which offers a mutation interface for +// adding and modifying parameters and residuals. The Program contains the core +// part of the Problem, which is the parameters and the residuals, stored in a +// particular ordering. The ordering is critical, since it defines the mapping +// between (residual, parameter) pairs and a position in the jacobian of the +// objective function. Various parts of Ceres transform one Program into +// another; for example, the first stage of solving involves stripping all +// constant parameters and residuals. This is in contrast with Problem, which is +// not built for transformation. +class Program { + public: + Program(); + explicit Program(const Program& program); + + // The ordered parameter and residual blocks for the program. + const std::vector<ParameterBlock*>& parameter_blocks() const; + const std::vector<ResidualBlock*>& residual_blocks() const; + std::vector<ParameterBlock*>* mutable_parameter_blocks(); + std::vector<ResidualBlock*>* mutable_residual_blocks(); + + // Serialize to/from the program and update states. + // + // NOTE: Setting the state of a parameter block can trigger the + // computation of the Jacobian of its local parameterization. If + // this computation fails for some reason, then this method returns + // false and the state of the parameter blocks cannot be trusted. + bool StateVectorToParameterBlocks(const double *state); + void ParameterBlocksToStateVector(double *state) const; + + // Copy internal state to the user's parameters. + void CopyParameterBlockStateToUserState(); + + // Set the parameter block pointers to the user pointers. Since this + // runs parameter block set state internally, which may call local + // parameterizations, this can fail. False is returned on failure. + bool SetParameterBlockStatePtrsToUserStatePtrs(); + + // Update a state vector for the program given a delta. + bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const; + + // Set the parameter indices and offsets. This permits mapping backward + // from a ParameterBlock* to an index in the parameter_blocks() vector. For + // any parameter block p, after calling SetParameterOffsetsAndIndex(), it + // is true that + // + // parameter_blocks()[p->index()] == p + // + // If a parameter appears in a residual but not in the parameter block, then + // it will have an index of -1. + // + // This also updates p->state_offset() and p->delta_offset(), which are the + // position of the parameter in the state and delta vector respectively. + void SetParameterOffsetsAndIndex(); + + // Check if the internal state of the program (the indexing and the + // offsets) are correct. + bool IsValid() const; + + bool ParameterBlocksAreFinite(std::string* message) const; + + // Returns true if the program has any non-constant parameter blocks + // which have non-trivial bounds constraints. + bool IsBoundsConstrained() const; + + // Returns false, if the program has any constant parameter blocks + // which are not feasible, or any variable parameter blocks which + // have a lower bound greater than or equal to the upper bound. + bool IsFeasible(std::string* message) const; + + // Loop over each residual block and ensure that no two parameter + // blocks in the same residual block are part of + // parameter_blocks as that would violate the assumption that it + // is an independent set in the Hessian matrix. + bool IsParameterBlockSetIndependent( + const std::set<double*>& independent_set) const; + + // 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. + TripletSparseMatrix* CreateJacobianBlockSparsityTranspose() const; + + // Create a copy of this program and removes constant parameter + // blocks and residual blocks with no varying parameter blocks while + // preserving their relative order. + // + // removed_parameter_blocks on exit will contain the list of + // parameter blocks that were removed. + // + // fixed_cost will be equal to the sum of the costs of the residual + // blocks that were removed. + // + // If there was a problem, then the function will return a NULL + // pointer and error will contain a human readable description of + // the problem. + Program* CreateReducedProgram(std::vector<double*>* removed_parameter_blocks, + double* fixed_cost, + std::string* error) const; + + // See problem.h for what these do. + int NumParameterBlocks() const; + int NumParameters() const; + int NumEffectiveParameters() const; + int NumResidualBlocks() const; + int NumResiduals() const; + + int MaxScratchDoublesNeededForEvaluate() const; + int MaxDerivativesPerResidualBlock() const; + int MaxParametersPerResidualBlock() const; + int MaxResidualsPerResidualBlock() const; + + // A human-readable dump of the parameter blocks for debugging. + // TODO(keir): If necessary, also dump the residual blocks. + std::string ToString() const; + + private: + // Remove constant parameter blocks and residual blocks with no + // varying parameter blocks while preserving their relative order. + // + // removed_parameter_blocks on exit will contain the list of + // parameter blocks that were removed. + // + // fixed_cost will be equal to the sum of the costs of the residual + // blocks that were removed. + // + // If there was a problem, then the function will return false and + // error will contain a human readable description of the problem. + bool RemoveFixedBlocks(std::vector<double*>* removed_parameter_blocks, + double* fixed_cost, + std::string* message); + + // The Program does not own the ParameterBlock or ResidualBlock objects. + std::vector<ParameterBlock*> parameter_blocks_; + std::vector<ResidualBlock*> residual_blocks_; + + friend class ProblemImpl; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PROGRAM_H_ diff --git a/extern/ceres/internal/ceres/program_evaluator.h b/extern/ceres/internal/ceres/program_evaluator.h new file mode 100644 index 00000000000..74a812adeef --- /dev/null +++ b/extern/ceres/internal/ceres/program_evaluator.h @@ -0,0 +1,384 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// The ProgramEvaluator runs the cost functions contained in each residual block +// and stores the result into a jacobian. The particular type of jacobian is +// abstracted out using two template parameters: +// +// - An "EvaluatePreparer" that is responsible for creating the array with +// pointers to the jacobian blocks where the cost function evaluates to. +// - A "JacobianWriter" that is responsible for storing the resulting +// jacobian blocks in the passed sparse matrix. +// +// This abstraction affords an efficient evaluator implementation while still +// supporting writing to multiple sparse matrix formats. For example, when the +// ProgramEvaluator is parameterized for writing to block sparse matrices, the +// residual jacobians are written directly into their final position in the +// block sparse matrix by the user's CostFunction; there is no copying. +// +// The evaluation is threaded with OpenMP. +// +// The EvaluatePreparer and JacobianWriter interfaces are as follows: +// +// class EvaluatePreparer { +// // Prepare the jacobians array for use as the destination of a call to +// // a cost function's evaluate method. +// void Prepare(const ResidualBlock* residual_block, +// int residual_block_index, +// SparseMatrix* jacobian, +// double** jacobians); +// } +// +// class JacobianWriter { +// // Create a jacobian that this writer can write. Same as +// // Evaluator::CreateJacobian. +// SparseMatrix* CreateJacobian() const; +// +// // Create num_threads evaluate preparers. Caller owns result which must +// // be freed with delete[]. Resulting preparers are valid while *this is. +// EvaluatePreparer* CreateEvaluatePreparers(int num_threads); +// +// // Write the block jacobians from a residual block evaluation to the +// // larger sparse jacobian. +// void Write(int residual_id, +// int residual_offset, +// double** jacobians, +// SparseMatrix* jacobian); +// } +// +// Note: The ProgramEvaluator is not thread safe, since internally it maintains +// some per-thread scratch space. + +#ifndef CERES_INTERNAL_PROGRAM_EVALUATOR_H_ +#define CERES_INTERNAL_PROGRAM_EVALUATOR_H_ + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +#include <map> +#include <string> +#include <vector> +#include "ceres/execution_summary.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/small_blas.h" + +namespace ceres { +namespace internal { + +struct NullJacobianFinalizer { + void operator()(SparseMatrix* jacobian, int num_parameters) {} +}; + +template<typename EvaluatePreparer, + typename JacobianWriter, + typename JacobianFinalizer = NullJacobianFinalizer> +class ProgramEvaluator : public Evaluator { + public: + ProgramEvaluator(const Evaluator::Options &options, Program* program) + : options_(options), + program_(program), + jacobian_writer_(options, program), + evaluate_preparers_( + jacobian_writer_.CreateEvaluatePreparers(options.num_threads)) { +#ifndef CERES_USE_OPENMP + if (options_.num_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads = 1 is supported. Switching " + << "to single threaded mode."; + options_.num_threads = 1; + } +#endif + + BuildResidualLayout(*program, &residual_layout_); + evaluate_scratch_.reset(CreateEvaluatorScratch(*program, + options.num_threads)); + } + + // Implementation of Evaluator interface. + SparseMatrix* CreateJacobian() const { + return jacobian_writer_.CreateJacobian(); + } + + bool Evaluate(const Evaluator::EvaluateOptions& evaluate_options, + const double* state, + double* cost, + double* residuals, + double* gradient, + SparseMatrix* jacobian) { + ScopedExecutionTimer total_timer("Evaluator::Total", &execution_summary_); + ScopedExecutionTimer call_type_timer(gradient == NULL && jacobian == NULL + ? "Evaluator::Residual" + : "Evaluator::Jacobian", + &execution_summary_); + + // The parameters are stateful, so set the state before evaluating. + if (!program_->StateVectorToParameterBlocks(state)) { + return false; + } + + if (residuals != NULL) { + VectorRef(residuals, program_->NumResiduals()).setZero(); + } + + if (jacobian != NULL) { + jacobian->SetZero(); + } + + // Each thread gets it's own cost and evaluate scratch space. + for (int i = 0; i < options_.num_threads; ++i) { + evaluate_scratch_[i].cost = 0.0; + if (gradient != NULL) { + VectorRef(evaluate_scratch_[i].gradient.get(), + program_->NumEffectiveParameters()).setZero(); + } + } + + // This bool is used to disable the loop if an error is encountered + // without breaking out of it. The remaining loop iterations are still run, + // but with an empty body, and so will finish quickly. + bool abort = false; + int num_residual_blocks = program_->NumResidualBlocks(); +#pragma omp parallel for num_threads(options_.num_threads) + for (int i = 0; i < num_residual_blocks; ++i) { +// Disable the loop instead of breaking, as required by OpenMP. +#pragma omp flush(abort) + if (abort) { + continue; + } + +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + EvaluatePreparer* preparer = &evaluate_preparers_[thread_id]; + EvaluateScratch* scratch = &evaluate_scratch_[thread_id]; + + // Prepare block residuals if requested. + const ResidualBlock* residual_block = program_->residual_blocks()[i]; + double* block_residuals = NULL; + if (residuals != NULL) { + block_residuals = residuals + residual_layout_[i]; + } else if (gradient != NULL) { + block_residuals = scratch->residual_block_residuals.get(); + } + + // Prepare block jacobians if requested. + double** block_jacobians = NULL; + if (jacobian != NULL || gradient != NULL) { + preparer->Prepare(residual_block, + i, + jacobian, + scratch->jacobian_block_ptrs.get()); + block_jacobians = scratch->jacobian_block_ptrs.get(); + } + + // Evaluate the cost, residuals, and jacobians. + double block_cost; + if (!residual_block->Evaluate( + evaluate_options.apply_loss_function, + &block_cost, + block_residuals, + block_jacobians, + scratch->residual_block_evaluate_scratch.get())) { + abort = true; +// This ensures that the OpenMP threads have a consistent view of 'abort'. Do +// the flush inside the failure case so that there is usually only one +// synchronization point per loop iteration instead of two. +#pragma omp flush(abort) + continue; + } + + scratch->cost += block_cost; + + // Store the jacobians, if they were requested. + if (jacobian != NULL) { + jacobian_writer_.Write(i, + residual_layout_[i], + block_jacobians, + jacobian); + } + + // Compute and store the gradient, if it was requested. + if (gradient != NULL) { + int num_residuals = residual_block->NumResiduals(); + int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (parameter_block->IsConstant()) { + continue; + } + + MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + block_jacobians[j], + num_residuals, + parameter_block->LocalSize(), + block_residuals, + scratch->gradient.get() + parameter_block->delta_offset()); + } + } + } + + if (!abort) { + const int num_parameters = program_->NumEffectiveParameters(); + + // Sum the cost and gradient (if requested) from each thread. + (*cost) = 0.0; + if (gradient != NULL) { + VectorRef(gradient, num_parameters).setZero(); + } + for (int i = 0; i < options_.num_threads; ++i) { + (*cost) += evaluate_scratch_[i].cost; + if (gradient != NULL) { + VectorRef(gradient, num_parameters) += + VectorRef(evaluate_scratch_[i].gradient.get(), num_parameters); + } + } + + // Finalize the Jacobian if it is available. + // `num_parameters` is passed to the finalizer so that additional + // storage can be reserved for additional diagonal elements if + // necessary. + if (jacobian != NULL) { + JacobianFinalizer f; + f(jacobian, num_parameters); + } + } + return !abort; + } + + bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const { + return program_->Plus(state, delta, state_plus_delta); + } + + int NumParameters() const { + return program_->NumParameters(); + } + int NumEffectiveParameters() const { + return program_->NumEffectiveParameters(); + } + + int NumResiduals() const { + return program_->NumResiduals(); + } + + virtual std::map<std::string, int> CallStatistics() const { + return execution_summary_.calls(); + } + + virtual std::map<std::string, double> TimeStatistics() const { + return execution_summary_.times(); + } + + private: + // Per-thread scratch space needed to evaluate and store each residual block. + struct EvaluateScratch { + void Init(int max_parameters_per_residual_block, + int max_scratch_doubles_needed_for_evaluate, + int max_residuals_per_residual_block, + int num_parameters) { + residual_block_evaluate_scratch.reset( + new double[max_scratch_doubles_needed_for_evaluate]); + gradient.reset(new double[num_parameters]); + VectorRef(gradient.get(), num_parameters).setZero(); + residual_block_residuals.reset( + new double[max_residuals_per_residual_block]); + jacobian_block_ptrs.reset( + new double*[max_parameters_per_residual_block]); + } + + double cost; + scoped_array<double> residual_block_evaluate_scratch; + // The gradient in the local parameterization. + scoped_array<double> gradient; + // Enough space to store the residual for the largest residual block. + scoped_array<double> residual_block_residuals; + scoped_array<double*> jacobian_block_ptrs; + }; + + static void BuildResidualLayout(const Program& program, + std::vector<int>* residual_layout) { + const std::vector<ResidualBlock*>& residual_blocks = + program.residual_blocks(); + residual_layout->resize(program.NumResidualBlocks()); + int residual_pos = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + const int num_residuals = residual_blocks[i]->NumResiduals(); + (*residual_layout)[i] = residual_pos; + residual_pos += num_residuals; + } + } + + // Create scratch space for each thread evaluating the program. + static EvaluateScratch* CreateEvaluatorScratch(const Program& program, + int num_threads) { + int max_parameters_per_residual_block = + program.MaxParametersPerResidualBlock(); + int max_scratch_doubles_needed_for_evaluate = + program.MaxScratchDoublesNeededForEvaluate(); + int max_residuals_per_residual_block = + program.MaxResidualsPerResidualBlock(); + int num_parameters = program.NumEffectiveParameters(); + + EvaluateScratch* evaluate_scratch = new EvaluateScratch[num_threads]; + for (int i = 0; i < num_threads; i++) { + evaluate_scratch[i].Init(max_parameters_per_residual_block, + max_scratch_doubles_needed_for_evaluate, + max_residuals_per_residual_block, + num_parameters); + } + return evaluate_scratch; + } + + Evaluator::Options options_; + Program* program_; + JacobianWriter jacobian_writer_; + scoped_array<EvaluatePreparer> evaluate_preparers_; + scoped_array<EvaluateScratch> evaluate_scratch_; + std::vector<int> residual_layout_; + ::ceres::internal::ExecutionSummary execution_summary_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PROGRAM_EVALUATOR_H_ diff --git a/extern/ceres/internal/ceres/random.h b/extern/ceres/internal/ceres/random.h new file mode 100644 index 00000000000..2a025600609 --- /dev/null +++ b/extern/ceres/internal/ceres/random.h @@ -0,0 +1,70 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_RANDOM_H_ +#define CERES_INTERNAL_RANDOM_H_ + +#include <cmath> +#include <cstdlib> +#include "ceres/internal/port.h" + +namespace ceres { + +inline void SetRandomState(int state) { + srand(state); +} + +inline int Uniform(int n) { + return rand() % n; +} + +inline double RandDouble() { + double r = static_cast<double>(rand()); + return r / RAND_MAX; +} + +// Box-Muller algorithm for normal random number generation. +// http://en.wikipedia.org/wiki/Box-Muller_transform +inline double RandNormal() { + double x1, x2, w; + do { + x1 = 2.0 * RandDouble() - 1.0; + x2 = 2.0 * RandDouble() - 1.0; + w = x1 * x1 + x2 * x2; + } while ( w >= 1.0 || w == 0.0 ); + + w = sqrt((-2.0 * log(w)) / w); + return x1 * w; +} + +} // namespace ceres + +#endif // CERES_INTERNAL_RANDOM_H_ diff --git a/extern/ceres/internal/ceres/reorder_program.cc b/extern/ceres/internal/ceres/reorder_program.cc new file mode 100644 index 00000000000..d0e8f32b3b7 --- /dev/null +++ b/extern/ceres/internal/ceres/reorder_program.cc @@ -0,0 +1,596 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/reorder_program.h" + +#include <algorithm> +#include <numeric> +#include <vector> + +#include "ceres/cxsparse.h" +#include "ceres/internal/port.h" +#include "ceres/ordered_groups.h" +#include "ceres/parameter_block.h" +#include "ceres/parameter_block_ordering.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/solver.h" +#include "ceres/suitesparse.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "Eigen/SparseCore" + +#ifdef CERES_USE_EIGEN_SPARSE +#include "Eigen/OrderingMethods" +#endif + +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::map; +using std::set; +using std::string; +using std::vector; + +namespace { + +// Find the minimum index of any parameter block to the given +// residual. Parameter blocks that have indices greater than +// size_of_first_elimination_group are considered to have an index +// equal to size_of_first_elimination_group. +static int MinParameterBlock(const ResidualBlock* residual_block, + int size_of_first_elimination_group) { + int min_parameter_block_position = size_of_first_elimination_group; + for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[i]; + if (!parameter_block->IsConstant()) { + CHECK_NE(parameter_block->index(), -1) + << "Did you forget to call Program::SetParameterOffsetsAndIndex()? " + << "This is a Ceres bug; please contact the developers!"; + min_parameter_block_position = std::min(parameter_block->index(), + min_parameter_block_position); + } + } + return min_parameter_block_position; +} + +#if EIGEN_VERSION_AT_LEAST(3, 2, 2) && defined(CERES_USE_EIGEN_SPARSE) +Eigen::SparseMatrix<int> CreateBlockJacobian( + const TripletSparseMatrix& block_jacobian_transpose) { + typedef Eigen::SparseMatrix<int> SparseMatrix; + typedef Eigen::Triplet<int> Triplet; + + const int* rows = block_jacobian_transpose.rows(); + const int* cols = block_jacobian_transpose.cols(); + int num_nonzeros = block_jacobian_transpose.num_nonzeros(); + vector<Triplet> triplets; + triplets.reserve(num_nonzeros); + for (int i = 0; i < num_nonzeros; ++i) { + triplets.push_back(Triplet(cols[i], rows[i], 1)); + } + + SparseMatrix block_jacobian(block_jacobian_transpose.num_cols(), + block_jacobian_transpose.num_rows()); + block_jacobian.setFromTriplets(triplets.begin(), triplets.end()); + return block_jacobian; +} +#endif + +void OrderingForSparseNormalCholeskyUsingSuiteSparse( + const TripletSparseMatrix& tsm_block_jacobian_transpose, + const vector<ParameterBlock*>& parameter_blocks, + const ParameterBlockOrdering& parameter_block_ordering, + int* ordering) { +#ifdef CERES_NO_SUITESPARSE + LOG(FATAL) << "Congratulations, you found a Ceres bug! " + << "Please report this error to the developers."; +#else + SuiteSparse ss; + cholmod_sparse* block_jacobian_transpose = + ss.CreateSparseMatrix( + const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose)); + + // No CAMD or the user did not supply a useful ordering, then just + // use regular AMD. + if (parameter_block_ordering.NumGroups() <= 1 || + !SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) { + ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]); + } else { + 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())); + } + + // 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]. + MapValuesToContiguousRange(constraints.size(), &constraints[0]); + ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose, + &constraints[0], + ordering); + } + + ss.Free(block_jacobian_transpose); +#endif // CERES_NO_SUITESPARSE +} + +void OrderingForSparseNormalCholeskyUsingCXSparse( + const TripletSparseMatrix& tsm_block_jacobian_transpose, + int* ordering) { +#ifdef CERES_NO_CXSPARSE + LOG(FATAL) << "Congratulations, you found a Ceres bug! " + << "Please report this error to the developers."; +#else // 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( + const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose)); + 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); + cxsparse.Free(block_hessian); +#endif // CERES_NO_CXSPARSE +} + + +#if EIGEN_VERSION_AT_LEAST(3, 2, 2) +void OrderingForSparseNormalCholeskyUsingEigenSparse( + const TripletSparseMatrix& tsm_block_jacobian_transpose, + int* ordering) { +#ifndef CERES_USE_EIGEN_SPARSE + LOG(FATAL) << + "SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE " + "because Ceres was not built with support for " + "Eigen's SimplicialLDLT decomposition. " + "This requires enabling building with -DEIGENSPARSE=ON."; +#else + + // This conversion from a TripletSparseMatrix to a Eigen::Triplet + // matrix is unfortunate, but unavoidable for now. It is not a + // significant performance penalty in the grand scheme of + // things. The right thing to do here would be to get a compressed + // row sparse matrix representation of the jacobian and go from + // there. But that is a project for another day. + typedef Eigen::SparseMatrix<int> SparseMatrix; + + const SparseMatrix block_jacobian = + CreateBlockJacobian(tsm_block_jacobian_transpose); + const SparseMatrix block_hessian = + block_jacobian.transpose() * block_jacobian; + + Eigen::AMDOrdering<int> amd_ordering; + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> perm; + amd_ordering(block_hessian, perm); + for (int i = 0; i < block_hessian.rows(); ++i) { + ordering[i] = perm.indices()[i]; + } +#endif // CERES_USE_EIGEN_SPARSE +} +#endif + +} // namespace + +bool ApplyOrdering(const ProblemImpl::ParameterMap& parameter_map, + const ParameterBlockOrdering& ordering, + Program* program, + string* error) { + const int num_parameter_blocks = program->NumParameterBlocks(); + if (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, + 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; +} + +bool LexicographicallyOrderResidualBlocks( + const int size_of_first_elimination_group, + Program* program, + string* error) { + CHECK_GE(size_of_first_elimination_group, 1) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Create a histogram of the number of residuals for each E block. There is an + // extra bucket at the end to catch all non-eliminated F blocks. + vector<int> residual_blocks_per_e_block(size_of_first_elimination_group + 1); + vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks(); + vector<int> min_position_per_residual(residual_blocks->size()); + for (int i = 0; i < residual_blocks->size(); ++i) { + ResidualBlock* residual_block = (*residual_blocks)[i]; + int position = MinParameterBlock(residual_block, + size_of_first_elimination_group); + min_position_per_residual[i] = position; + DCHECK_LE(position, size_of_first_elimination_group); + residual_blocks_per_e_block[position]++; + } + + // Run a cumulative sum on the histogram, to obtain offsets to the start of + // each histogram bucket (where each bucket is for the residuals for that + // E-block). + vector<int> offsets(size_of_first_elimination_group + 1); + std::partial_sum(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end(), + offsets.begin()); + CHECK_EQ(offsets.back(), residual_blocks->size()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + CHECK(find(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end() - 1, 0) != + residual_blocks_per_e_block.end()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Fill in each bucket with the residual blocks for its corresponding E block. + // Each bucket is individually filled from the back of the bucket to the front + // of the bucket. The filling order among the buckets is dictated by the + // residual blocks. This loop uses the offsets as counters; subtracting one + // from each offset as a residual block is placed in the bucket. When the + // filling is finished, the offset pointerts should have shifted down one + // entry (this is verified below). + vector<ResidualBlock*> reordered_residual_blocks( + (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL)); + for (int i = 0; i < residual_blocks->size(); ++i) { + int bucket = min_position_per_residual[i]; + + // Decrement the cursor, which should now point at the next empty position. + offsets[bucket]--; + + // Sanity. + CHECK(reordered_residual_blocks[offsets[bucket]] == NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i]; + } + + // Sanity check #1: The difference in bucket offsets should match the + // histogram sizes. + for (int i = 0; i < size_of_first_elimination_group; ++i) { + CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i]) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + // Sanity check #2: No NULL's left behind. + for (int i = 0; i < reordered_residual_blocks.size(); ++i) { + CHECK(reordered_residual_blocks[i] != NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + + // Now that the residuals are collected by E block, swap them in place. + swap(*program->mutable_residual_blocks(), reordered_residual_blocks); + return true; +} + +// Pre-order the columns corresponding to the schur complement if +// possible. +void MaybeReorderSchurComplementColumnsUsingSuiteSparse( + const ParameterBlockOrdering& parameter_block_ordering, + Program* program) { +#ifndef CERES_NO_SUITESPARSE + SuiteSparse ss; + if (!SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) { + return; + } + + 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]. + MapValuesToContiguousRange(constraints.size(), &constraints[0]); + + // Compute a block sparse presentation of J'. + scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( + program->CreateJacobianBlockSparsityTranspose()); + + 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]]; + } + + program->SetParameterOffsetsAndIndex(); +#endif +} + +void MaybeReorderSchurComplementColumnsUsingEigen( + const int size_of_first_elimination_group, + const ProblemImpl::ParameterMap& parameter_map, + Program* program) { +#if !EIGEN_VERSION_AT_LEAST(3, 2, 2) || !defined(CERES_USE_EIGEN_SPARSE) + return; +#else + + scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( + program->CreateJacobianBlockSparsityTranspose()); + + typedef Eigen::SparseMatrix<int> SparseMatrix; + const SparseMatrix block_jacobian = + CreateBlockJacobian(*tsm_block_jacobian_transpose); + const int num_rows = block_jacobian.rows(); + const int num_cols = block_jacobian.cols(); + + // Vertically partition the jacobian in parameter blocks of type E + // and F. + const SparseMatrix E = + block_jacobian.block(0, + 0, + num_rows, + size_of_first_elimination_group); + const SparseMatrix F = + block_jacobian.block(0, + size_of_first_elimination_group, + num_rows, + num_cols - size_of_first_elimination_group); + + // Block sparsity pattern of the schur complement. + const SparseMatrix block_schur_complement = + F.transpose() * F - F.transpose() * E * E.transpose() * F; + + Eigen::AMDOrdering<int> amd_ordering; + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> perm; + amd_ordering(block_schur_complement, perm); + + const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); + vector<ParameterBlock*> ordering(num_cols); + + // The ordering of the first size_of_first_elimination_group does + // not matter, so we preserve the existing ordering. + for (int i = 0; i < size_of_first_elimination_group; ++i) { + ordering[i] = parameter_blocks[i]; + } + + // For the rest of the blocks, use the ordering computed using AMD. + for (int i = 0; i < block_schur_complement.cols(); ++i) { + ordering[size_of_first_elimination_group + i] = + parameter_blocks[size_of_first_elimination_group + perm.indices()[i]]; + } + + swap(*program->mutable_parameter_blocks(), ordering); + program->SetParameterOffsetsAndIndex(); +#endif +} + +bool 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->NumElements() != + program->NumParameterBlocks()) { + *error = StringPrintf( + "The program has %d parameter blocks, but the parameter block " + "ordering has %d parameter blocks.", + program->NumParameterBlocks(), + parameter_block_ordering->NumElements()); + return false; + } + + 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 size_of_first_elimination_group = + 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 < size_of_first_elimination_group) ? 0 : 1; + parameter_block_ordering->AddElementToGroup(parameter_block, group_id); + } + + // We could call ApplyOrdering 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. + + // Verify that the first elimination group is an independent set. + const set<double*>& first_elimination_group = + parameter_block_ordering + ->group_to_elements() + .begin() + ->second; + if (!program->IsParameterBlockSetIndependent(first_elimination_group)) { + *error = + StringPrintf("The first elimination group in the parameter block " + "ordering of size %zd is not an independent set", + first_elimination_group.size()); + return false; + } + + if (!ApplyOrdering(parameter_map, + *parameter_block_ordering, + program, + error)) { + return false; + } + } + + program->SetParameterOffsetsAndIndex(); + + const int size_of_first_elimination_group = + parameter_block_ordering->group_to_elements().begin()->second.size(); + + if (linear_solver_type == SPARSE_SCHUR) { + if (sparse_linear_algebra_library_type == SUITE_SPARSE) { + MaybeReorderSchurComplementColumnsUsingSuiteSparse( + *parameter_block_ordering, + program); + } else if (sparse_linear_algebra_library_type == EIGEN_SPARSE) { + MaybeReorderSchurComplementColumnsUsingEigen( + size_of_first_elimination_group, + parameter_map, + program); + } + } + + // Schur type solvers also require that their residual blocks be + // lexicographically ordered. + if (!LexicographicallyOrderResidualBlocks(size_of_first_elimination_group, + program, + error)) { + return false; + } + + return true; +} + +bool ReorderProgramForSparseNormalCholesky( + const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ParameterBlockOrdering& parameter_block_ordering, + Program* program, + string* error) { + if (parameter_block_ordering.NumElements() != program->NumParameterBlocks()) { + *error = StringPrintf( + "The program has %d parameter blocks, but the parameter block " + "ordering has %d parameter blocks.", + program->NumParameterBlocks(), + parameter_block_ordering.NumElements()); + return false; + } + + // Compute a block sparse presentation of J'. + scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( + program->CreateJacobianBlockSparsityTranspose()); + + vector<int> ordering(program->NumParameterBlocks(), 0); + vector<ParameterBlock*>& parameter_blocks = + *(program->mutable_parameter_blocks()); + + if (sparse_linear_algebra_library_type == SUITE_SPARSE) { + OrderingForSparseNormalCholeskyUsingSuiteSparse( + *tsm_block_jacobian_transpose, + parameter_blocks, + parameter_block_ordering, + &ordering[0]); + } else if (sparse_linear_algebra_library_type == CX_SPARSE) { + OrderingForSparseNormalCholeskyUsingCXSparse( + *tsm_block_jacobian_transpose, + &ordering[0]); + } else if (sparse_linear_algebra_library_type == EIGEN_SPARSE) { +#if EIGEN_VERSION_AT_LEAST(3, 2, 2) + OrderingForSparseNormalCholeskyUsingEigenSparse( + *tsm_block_jacobian_transpose, + &ordering[0]); +#else + // For Eigen versions less than 3.2.2, there is nothing to do as + // older versions of Eigen do not expose a method for doing + // symbolic analysis on pre-ordered matrices, so a block + // pre-ordering is a bit pointless. + + return true; +#endif + } + + // Apply ordering. + const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); + for (int i = 0; i < program->NumParameterBlocks(); ++i) { + parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; + } + + program->SetParameterOffsetsAndIndex(); + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/reorder_program.h b/extern/ceres/internal/ceres/reorder_program.h new file mode 100644 index 00000000000..36e5d1637a9 --- /dev/null +++ b/extern/ceres/internal/ceres/reorder_program.h @@ -0,0 +1,101 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_REORDER_PROGRAM_H_ +#define CERES_INTERNAL_REORDER_PROGRAM_H_ + +#include <string> +#include "ceres/internal/port.h" +#include "ceres/parameter_block_ordering.h" +#include "ceres/problem_impl.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class Program; + +// Reorder the parameter blocks in program using the ordering +bool ApplyOrdering(const ProblemImpl::ParameterMap& parameter_map, + const ParameterBlockOrdering& ordering, + Program* program, + std::string* error); + +// Reorder the residuals for program, if necessary, so that the residuals +// involving each E block occur together. This is a necessary condition for the +// Schur eliminator, which works on these "row blocks" in the jacobian. +bool LexicographicallyOrderResidualBlocks(int size_of_first_elimination_group, + Program* program, + std::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 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. +bool ReorderProgramForSchurTypeLinearSolver( + LinearSolverType linear_solver_type, + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ProblemImpl::ParameterMap& parameter_map, + ParameterBlockOrdering* parameter_block_ordering, + Program* program, + std::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. +// +// When using SuiteSparse, 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. +bool ReorderProgramForSparseNormalCholesky( + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ParameterBlockOrdering& parameter_block_ordering, + Program* program, + std::string* error); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_REORDER_PROGRAM_ diff --git a/extern/ceres/internal/ceres/residual_block.cc b/extern/ceres/internal/ceres/residual_block.cc new file mode 100644 index 00000000000..9a123cf132e --- /dev/null +++ b/extern/ceres/internal/ceres/residual_block.cc @@ -0,0 +1,219 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/residual_block.h" + +#include <algorithm> +#include <cstddef> +#include <vector> +#include "ceres/corrector.h" +#include "ceres/parameter_block.h" +#include "ceres/residual_block_utils.h" +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/local_parameterization.h" +#include "ceres/loss_function.h" +#include "ceres/small_blas.h" + +using Eigen::Dynamic; + +namespace ceres { +namespace internal { + +ResidualBlock::ResidualBlock( + const CostFunction* cost_function, + const LossFunction* loss_function, + const std::vector<ParameterBlock*>& parameter_blocks, + int index) + : cost_function_(cost_function), + loss_function_(loss_function), + parameter_blocks_( + new ParameterBlock* [ + cost_function->parameter_block_sizes().size()]), + index_(index) { + std::copy(parameter_blocks.begin(), + parameter_blocks.end(), + parameter_blocks_.get()); +} + +bool ResidualBlock::Evaluate(const bool apply_loss_function, + double* cost, + double* residuals, + double** jacobians, + double* scratch) const { + const int num_parameter_blocks = NumParameterBlocks(); + const int num_residuals = cost_function_->num_residuals(); + + // Collect the parameters from their blocks. This will rarely allocate, since + // residuals taking more than 8 parameter block arguments are rare. + FixedArray<const double*, 8> parameters(num_parameter_blocks); + for (int i = 0; i < num_parameter_blocks; ++i) { + parameters[i] = parameter_blocks_[i]->state(); + } + + // Put pointers into the scratch space into global_jacobians as appropriate. + FixedArray<double*, 8> global_jacobians(num_parameter_blocks); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + if (jacobians[i] != NULL && + parameter_block->LocalParameterizationJacobian() != NULL) { + global_jacobians[i] = scratch; + scratch += num_residuals * parameter_block->Size(); + } else { + global_jacobians[i] = jacobians[i]; + } + } + } + + // If the caller didn't request residuals, use the scratch space for them. + bool outputting_residuals = (residuals != NULL); + if (!outputting_residuals) { + residuals = scratch; + } + + // Invalidate the evaluation buffers so that we can check them after + // the CostFunction::Evaluate call, to see if all the return values + // that were required were written to and that they are finite. + double** eval_jacobians = (jacobians != NULL) ? global_jacobians.get() : NULL; + + InvalidateEvaluation(*this, cost, residuals, eval_jacobians); + + if (!cost_function_->Evaluate(parameters.get(), residuals, eval_jacobians)) { + return false; + } + + if (!IsEvaluationValid(*this, + parameters.get(), + cost, + residuals, + eval_jacobians)) { + std::string message = + "\n\n" + "Error in evaluating the ResidualBlock.\n\n" + "There are two possible reasons. Either the CostFunction did not evaluate and fill all \n" // NOLINT + "residual and jacobians that were requested or there was a non-finite value (nan/infinite)\n" // NOLINT + "generated during the or jacobian computation. \n\n" + + EvaluationToString(*this, + parameters.get(), + cost, + residuals, + eval_jacobians); + LOG(WARNING) << message; + return false; + } + + double squared_norm = VectorRef(residuals, num_residuals).squaredNorm(); + + // Update the jacobians with the local parameterizations. + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + if (jacobians[i] != NULL) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + + // Apply local reparameterization to the jacobians. + if (parameter_block->LocalParameterizationJacobian() != NULL) { + // jacobians[i] = global_jacobians[i] * global_to_local_jacobian. + MatrixMatrixMultiply<Dynamic, Dynamic, Dynamic, Dynamic, 0>( + global_jacobians[i], + num_residuals, + parameter_block->Size(), + parameter_block->LocalParameterizationJacobian(), + parameter_block->Size(), + parameter_block->LocalSize(), + jacobians[i], 0, 0, num_residuals, parameter_block->LocalSize()); + } + } + } + } + + if (loss_function_ == NULL || !apply_loss_function) { + *cost = 0.5 * squared_norm; + return true; + } + + double rho[3]; + loss_function_->Evaluate(squared_norm, rho); + *cost = 0.5 * rho[0]; + + // No jacobians and not outputting residuals? All done. Doing an early exit + // here avoids constructing the "Corrector" object below in a common case. + if (jacobians == NULL && !outputting_residuals) { + return true; + } + + // Correct for the effects of the loss function. The jacobians need to be + // corrected before the residuals, since they use the uncorrected residuals. + Corrector correct(squared_norm, rho); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + if (jacobians[i] != NULL) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + + // Correct the jacobians for the loss function. + correct.CorrectJacobian(num_residuals, + parameter_block->LocalSize(), + residuals, + jacobians[i]); + } + } + } + + // Correct the residuals with the loss function. + if (outputting_residuals) { + correct.CorrectResiduals(num_residuals, residuals); + } + return true; +} + +int ResidualBlock::NumScratchDoublesForEvaluate() const { + // Compute the amount of scratch space needed to store the full-sized + // jacobians. For parameters that have no local parameterization no storage + // is needed and the passed-in jacobian array is used directly. Also include + // space to store the residuals, which is needed for cost-only evaluations. + // This is slightly pessimistic, since both won't be needed all the time, but + // the amount of excess should not cause problems for the caller. + int num_parameters = NumParameterBlocks(); + int scratch_doubles = 1; + for (int i = 0; i < num_parameters; ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + if (!parameter_block->IsConstant() && + parameter_block->LocalParameterizationJacobian() != NULL) { + scratch_doubles += parameter_block->Size(); + } + } + scratch_doubles *= NumResiduals(); + return scratch_doubles; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/residual_block.h b/extern/ceres/internal/ceres/residual_block.h new file mode 100644 index 00000000000..05e6d1f81e5 --- /dev/null +++ b/extern/ceres/internal/ceres/residual_block.h @@ -0,0 +1,148 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) +// +// Purpose : Class and struct definitions for parameter and residual blocks. + +#ifndef CERES_INTERNAL_RESIDUAL_BLOCK_H_ +#define CERES_INTERNAL_RESIDUAL_BLOCK_H_ + +#include <string> +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/stringprintf.h" +#include "ceres/types.h" + +namespace ceres { + +class LossFunction; + +namespace internal { + +class ParameterBlock; + +// A term in the least squares problem. The mathematical form of each term in +// the overall least-squares cost function is: +// +// 1 +// --- loss_function( || cost_function(block1, block2, ...) ||^2 ), +// 2 +// +// Storing the cost function and the loss function separately permits optimizing +// the problem with standard non-linear least techniques, without requiring a +// more general non-linear solver. +// +// The residual block stores pointers to but does not own the cost functions, +// loss functions, and parameter blocks. +class ResidualBlock { + public: + // Construct the residual block with the given cost/loss functions. Loss may + // be null. The index is the index of the residual block in the Program's + // residual_blocks array. + ResidualBlock(const CostFunction* cost_function, + const LossFunction* loss_function, + const std::vector<ParameterBlock*>& parameter_blocks, + int index); + + // Evaluates the residual term, storing the scalar cost in *cost, the residual + // components in *residuals, and the jacobians between the parameters and + // residuals in jacobians[i], in row-major order. If residuals is NULL, the + // residuals are not computed. If jacobians is NULL, no jacobians are + // computed. If jacobians[i] is NULL, then the jacobian for that parameter is + // not computed. + // + // Evaluate needs scratch space which must be supplied by the caller via + // scratch. The array should have at least NumScratchDoublesForEvaluate() + // space available. + // + // The return value indicates the success or failure. If the function returns + // false, the caller should expect the the output memory locations to have + // been modified. + // + // The returned cost and jacobians have had robustification and local + // parameterizations applied already; for example, the jacobian for a + // 4-dimensional quaternion parameter using the "QuaternionParameterization" + // is num_residuals by 3 instead of num_residuals by 4. + // + // apply_loss_function as the name implies allows the user to switch + // the application of the loss function on and off. + bool Evaluate(bool apply_loss_function, + double* cost, + double* residuals, + double** jacobians, + double* scratch) const; + + + const CostFunction* cost_function() const { return cost_function_; } + const LossFunction* loss_function() const { return loss_function_; } + + // Access the parameter blocks for this residual. The array has size + // NumParameterBlocks(). + ParameterBlock* const* parameter_blocks() const { + return parameter_blocks_.get(); + } + + // Number of variable blocks that this residual term depends on. + int NumParameterBlocks() const { + return cost_function_->parameter_block_sizes().size(); + } + + // The size of the residual vector returned by this residual function. + int NumResiduals() const { return cost_function_->num_residuals(); } + + // The minimum amount of scratch space needed to pass to Evaluate(). + int NumScratchDoublesForEvaluate() const; + + // This residual block's index in an array. + int index() const { return index_; } + void set_index(int index) { index_ = index; } + + std::string ToString() { + return StringPrintf("{residual block; index=%d}", index_); + } + + private: + const CostFunction* cost_function_; + const LossFunction* loss_function_; + scoped_array<ParameterBlock*> parameter_blocks_; + + // The index of the residual, typically in a Program. This is only to permit + // switching from a ResidualBlock* to an index in the Program's array, needed + // to do efficient removals. + int32 index_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_RESIDUAL_BLOCK_H_ diff --git a/extern/ceres/internal/ceres/residual_block_utils.cc b/extern/ceres/internal/ceres/residual_block_utils.cc new file mode 100644 index 00000000000..dd2bd73a6ac --- /dev/null +++ b/extern/ceres/internal/ceres/residual_block_utils.cc @@ -0,0 +1,142 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/residual_block_utils.h" + +#include <cmath> +#include <cstddef> +#include <limits> +#include "ceres/array_utils.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/parameter_block.h" +#include "ceres/residual_block.h" +#include "ceres/stringprintf.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::string; + +void InvalidateEvaluation(const ResidualBlock& block, + double* cost, + double* residuals, + double** jacobians) { + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + + InvalidateArray(1, cost); + InvalidateArray(num_residuals, residuals); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + InvalidateArray(num_residuals * parameter_block_size, jacobians[i]); + } + } +} + +string EvaluationToString(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians) { + CHECK_NOTNULL(cost); + CHECK_NOTNULL(residuals); + + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + string result = ""; + + StringAppendF(&result, + "Residual Block size: %d parameter blocks x %d residuals\n\n", + num_parameter_blocks, num_residuals); + result += + "For each parameter block, the value of the parameters are printed in the first column \n" // NOLINT + "and the value of the jacobian under the corresponding residual. If a ParameterBlock was \n" // NOLINT + "held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry \n" // NOLINT + "of the Jacobian/residual array was requested but was not written to by user code, it is \n" // NOLINT + "indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating \n" // NOLINT + "to Inf or NaN is also an error. \n\n"; // NOLINT + + string space = "Residuals: "; + result += space; + AppendArrayToString(num_residuals, residuals, &result); + StringAppendF(&result, "\n\n"); + + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + StringAppendF( + &result, "Parameter Block %d, size: %d\n", i, parameter_block_size); + StringAppendF(&result, "\n"); + for (int j = 0; j < parameter_block_size; ++j) { + AppendArrayToString(1, parameters[i] + j, &result); + StringAppendF(&result, "| "); + for (int k = 0; k < num_residuals; ++k) { + AppendArrayToString(1, + (jacobians != NULL && jacobians[i] != NULL) + ? jacobians[i] + k * parameter_block_size + j + : NULL, + &result); + } + StringAppendF(&result, "\n"); + } + StringAppendF(&result, "\n"); + } + StringAppendF(&result, "\n"); + return result; +} + +bool IsEvaluationValid(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians) { + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + + if (!IsArrayValid(num_residuals, residuals)) { + return false; + } + + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + if (!IsArrayValid(num_residuals * parameter_block_size, jacobians[i])) { + return false; + } + } + } + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/residual_block_utils.h b/extern/ceres/internal/ceres/residual_block_utils.h new file mode 100644 index 00000000000..627337f743c --- /dev/null +++ b/extern/ceres/internal/ceres/residual_block_utils.h @@ -0,0 +1,80 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Utility routines for ResidualBlock evaluation. +// +// These are useful for detecting two common class of errors. +// +// 1. Uninitialized memory - where the user for some reason did not +// compute part of a cost/residual/jacobian. +// +// 2. Numerical failure while computing the cost/residual/jacobian, +// e.g. NaN, infinities etc. This is particularly useful since the +// automatic differentiation code does computations that are not +// evident to the user and can silently generate hard to debug errors. + +#ifndef CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ +#define CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ + +#include <string> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class ResidualBlock; + +// Invalidate cost, resdual and jacobian arrays (if not NULL). +void InvalidateEvaluation(const ResidualBlock& block, + double* cost, + double* residuals, + double** jacobians); + +// Check if any of the arrays cost, residuals or jacobians contains an +// NaN, return true if it does. +bool IsEvaluationValid(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians); + +// Create a string representation of the Residual block containing the +// value of the parameters, residuals and jacobians if present. +// Useful for debugging output. +std::string EvaluationToString(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ diff --git a/extern/ceres/internal/ceres/schur_complement_solver.cc b/extern/ceres/internal/ceres/schur_complement_solver.cc new file mode 100644 index 00000000000..2491060dcdc --- /dev/null +++ b/extern/ceres/internal/ceres/schur_complement_solver.cc @@ -0,0 +1,670 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/internal/port.h" + +#include <algorithm> +#include <ctime> +#include <set> +#include <vector> + +#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/conjugate_gradients_solver.h" +#include "ceres/cxsparse.h" +#include "ceres/detect_structure.h" +#include "ceres/internal/eigen.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" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "ceres/wall_time.h" +#include "Eigen/Dense" +#include "Eigen/SparseCore" + +namespace ceres { +namespace internal { + +using std::make_pair; +using std::pair; +using std::set; +using std::vector; + +namespace { + +class BlockRandomAccessSparseMatrixAdapter : public LinearOperator { + public: + explicit BlockRandomAccessSparseMatrixAdapter( + const BlockRandomAccessSparseMatrix& m) + : m_(m) { + } + + virtual ~BlockRandomAccessSparseMatrixAdapter() {} + + // y = y + Ax; + virtual void RightMultiply(const double* x, double* y) const { + m_.SymmetricRightMultiply(x, y); + } + + // y = y + A'x; + virtual void LeftMultiply(const double* x, double* y) const { + m_.SymmetricRightMultiply(x, y); + } + + virtual int num_rows() const { return m_.num_rows(); } + virtual int num_cols() const { return m_.num_rows(); } + + private: + const BlockRandomAccessSparseMatrix& m_; +}; + +class BlockRandomAccessDiagonalMatrixAdapter : public LinearOperator { + public: + explicit BlockRandomAccessDiagonalMatrixAdapter( + const BlockRandomAccessDiagonalMatrix& m) + : m_(m) { + } + + virtual ~BlockRandomAccessDiagonalMatrixAdapter() {} + + // y = y + Ax; + virtual void RightMultiply(const double* x, double* y) const { + m_.RightMultiply(x, y); + } + + // y = y + A'x; + virtual void LeftMultiply(const double* x, double* y) const { + m_.RightMultiply(x, y); + } + + virtual int num_rows() const { return m_.num_rows(); } + virtual int num_cols() const { return m_.num_rows(); } + + private: + const BlockRandomAccessDiagonalMatrix& m_; +}; + +} // namespace + +LinearSolver::Summary SchurComplementSolver::SolveImpl( + BlockSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + EventLogger event_logger("SchurComplementSolver::Solve"); + + if (eliminator_.get() == NULL) { + InitStorage(A->block_structure()); + DetectStructure(*A->block_structure(), + options_.elimination_groups[0], + &options_.row_block_size, + &options_.e_block_size, + &options_.f_block_size); + eliminator_.reset(CHECK_NOTNULL(SchurEliminatorBase::Create(options_))); + eliminator_->Init(options_.elimination_groups[0], A->block_structure()); + }; + std::fill(x, x + A->num_cols(), 0.0); + event_logger.AddEvent("Setup"); + + eliminator_->Eliminate(A, b, per_solve_options.D, lhs_.get(), rhs_.get()); + event_logger.AddEvent("Eliminate"); + + double* reduced_solution = x + A->num_cols() - lhs_->num_cols(); + const LinearSolver::Summary summary = + SolveReducedLinearSystem(per_solve_options, reduced_solution); + event_logger.AddEvent("ReducedSolve"); + + if (summary.termination_type == LINEAR_SOLVER_SUCCESS) { + eliminator_->BackSubstitute(A, b, per_solve_options.D, reduced_solution, x); + event_logger.AddEvent("BackSubstitute"); + } + + return summary; +} + +// Initialize a BlockRandomAccessDenseMatrix to store the Schur +// complement. +void DenseSchurComplementSolver::InitStorage( + const CompressedRowBlockStructure* bs) { + const int num_eliminate_blocks = options().elimination_groups[0]; + const int num_col_blocks = bs->cols.size(); + + vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0); + for (int i = num_eliminate_blocks, j = 0; + i < num_col_blocks; + ++i, ++j) { + blocks[j] = bs->cols[i].size; + } + + set_lhs(new BlockRandomAccessDenseMatrix(blocks)); + set_rhs(new double[lhs()->num_rows()]); +} + +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessDenseMatrix. The linear system is solved using +// Eigen's Cholesky factorization. +LinearSolver::Summary +DenseSchurComplementSolver::SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution) { + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + const BlockRandomAccessDenseMatrix* m = + down_cast<const BlockRandomAccessDenseMatrix*>(lhs()); + const int num_rows = m->num_rows(); + + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + return summary; + } + + summary.num_iterations = 1; + + if (options().dense_linear_algebra_library_type == EIGEN) { + Eigen::LLT<Matrix, Eigen::Upper> llt = + ConstMatrixRef(m->values(), num_rows, num_rows) + .selfadjointView<Eigen::Upper>() + .llt(); + if (llt.info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = + "Eigen failure. Unable to perform dense Cholesky factorization."; + return summary; + } + + VectorRef(solution, num_rows) = llt.solve(ConstVectorRef(rhs(), num_rows)); + } else { + VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows); + summary.termination_type = + LAPACK::SolveInPlaceUsingCholesky(num_rows, + m->values(), + solution, + &summary.message); + } + + return summary; +} + +SparseSchurComplementSolver::SparseSchurComplementSolver( + const LinearSolver::Options& options) + : SchurComplementSolver(options), + factor_(NULL), + cxsparse_factor_(NULL) { +} + +SparseSchurComplementSolver::~SparseSchurComplementSolver() { + if (factor_ != NULL) { + ss_.Free(factor_); + factor_ = NULL; + } + + if (cxsparse_factor_ != NULL) { + cxsparse_.Free(cxsparse_factor_); + cxsparse_factor_ = NULL; + } +} + +// Determine the non-zero blocks in the Schur Complement matrix, and +// initialize a BlockRandomAccessSparseMatrix object. +void SparseSchurComplementSolver::InitStorage( + const CompressedRowBlockStructure* bs) { + const int num_eliminate_blocks = options().elimination_groups[0]; + const int num_col_blocks = bs->cols.size(); + const int num_row_blocks = bs->rows.size(); + + blocks_.resize(num_col_blocks - num_eliminate_blocks, 0); + for (int i = num_eliminate_blocks; i < num_col_blocks; ++i) { + blocks_[i - num_eliminate_blocks] = bs->cols[i].size; + } + + set<pair<int, int> > block_pairs; + for (int i = 0; i < blocks_.size(); ++i) { + block_pairs.insert(make_pair(i, i)); + } + + int r = 0; + while (r < num_row_blocks) { + int e_block_id = bs->rows[r].cells.front().block_id; + if (e_block_id >= num_eliminate_blocks) { + break; + } + vector<int> f_blocks; + + // Add to the chunk until the first block in the row is + // different than the one in the first row for the chunk. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs->rows[r]; + if (row.cells.front().block_id != e_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first + // block since it is the one to be eliminated. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + f_blocks.push_back(cell.block_id - num_eliminate_blocks); + } + } + + sort(f_blocks.begin(), f_blocks.end()); + f_blocks.erase(unique(f_blocks.begin(), f_blocks.end()), f_blocks.end()); + for (int i = 0; i < f_blocks.size(); ++i) { + for (int j = i + 1; j < f_blocks.size(); ++j) { + block_pairs.insert(make_pair(f_blocks[i], f_blocks[j])); + } + } + } + + // Remaing rows do not contribute to the chunks and directly go + // into the schur complement via an outer product. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs->rows[r]; + CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); + for (int i = 0; i < row.cells.size(); ++i) { + int r_block1_id = row.cells[i].block_id - num_eliminate_blocks; + for (int j = 0; j < row.cells.size(); ++j) { + int r_block2_id = row.cells[j].block_id - num_eliminate_blocks; + if (r_block1_id <= r_block2_id) { + block_pairs.insert(make_pair(r_block1_id, r_block2_id)); + } + } + } + } + + set_lhs(new BlockRandomAccessSparseMatrix(blocks_, block_pairs)); + set_rhs(new double[lhs()->num_rows()]); +} + +LinearSolver::Summary +SparseSchurComplementSolver::SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution) { + if (options().type == ITERATIVE_SCHUR) { + CHECK(options().use_explicit_schur_complement); + return SolveReducedLinearSystemUsingConjugateGradients(per_solve_options, + solution); + } + + switch (options().sparse_linear_algebra_library_type) { + case SUITE_SPARSE: + return SolveReducedLinearSystemUsingSuiteSparse(per_solve_options, + solution); + case CX_SPARSE: + return SolveReducedLinearSystemUsingCXSparse(per_solve_options, + solution); + case EIGEN_SPARSE: + return SolveReducedLinearSystemUsingEigen(per_solve_options, + solution); + default: + LOG(FATAL) << "Unknown sparse linear algebra library : " + << options().sparse_linear_algebra_library_type; + } + + return LinearSolver::Summary(); +} + +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessSparseMatrix. The linear system is solved using +// CHOLMOD's sparse cholesky factorization routines. +LinearSolver::Summary +SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution) { +#ifdef CERES_NO_SUITESPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = "Ceres was not built with SuiteSparse support. " + "Therefore, SPARSE_SCHUR cannot be used with SUITE_SPARSE"; + return summary; + +#else + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + TripletSparseMatrix* tsm = + const_cast<TripletSparseMatrix*>( + down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); + const int num_rows = tsm->num_rows(); + + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + return summary; + } + + summary.num_iterations = 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. + + // Create a upper triangular symmetric matrix. + cholmod_lhs = ss_.CreateSparseMatrix(tsm); + cholmod_lhs->stype = 1; + + if (factor_ == NULL) { + factor_ = ss_.BlockAnalyzeCholesky(cholmod_lhs, + blocks_, + blocks_, + &summary.message); + } + } 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, + &summary.message); + } + } + + if (factor_ == NULL) { + ss_.Free(cholmod_lhs); + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + // No need to set message as it has already been set by the + // symbolic analysis routines above. + return summary; + } + + summary.termination_type = + ss_.Cholesky(cholmod_lhs, factor_, &summary.message); + + ss_.Free(cholmod_lhs); + + if (summary.termination_type != LINEAR_SOLVER_SUCCESS) { + // No need to set message as it has already been set by the + // numeric factorization routine above. + return summary; + } + + cholmod_dense* cholmod_rhs = + ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows); + cholmod_dense* cholmod_solution = ss_.Solve(factor_, + cholmod_rhs, + &summary.message); + ss_.Free(cholmod_rhs); + + if (cholmod_solution == NULL) { + summary.message = + "SuiteSparse failure. Unable to perform triangular solve."; + summary.termination_type = LINEAR_SOLVER_FAILURE; + return summary; + } + + VectorRef(solution, num_rows) + = VectorRef(static_cast<double*>(cholmod_solution->x), num_rows); + ss_.Free(cholmod_solution); + return summary; +#endif // CERES_NO_SUITESPARSE +} + +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessSparseMatrix. The linear system is solved using +// CXSparse's sparse cholesky factorization routines. +LinearSolver::Summary +SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution) { +#ifdef CERES_NO_CXSPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = "Ceres was not built with CXSparse support. " + "Therefore, SPARSE_SCHUR cannot be used with CX_SPARSE"; + return summary; + +#else + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + // Extract the TripletSparseMatrix that is used for actually storing S. + TripletSparseMatrix* tsm = + const_cast<TripletSparseMatrix*>( + down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); + const int num_rows = tsm->num_rows(); + + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + return summary; + } + + cs_di* lhs = CHECK_NOTNULL(cxsparse_.CreateSparseMatrix(tsm)); + VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows); + + // Compute symbolic factorization if not available. + if (cxsparse_factor_ == NULL) { + cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(lhs, blocks_, blocks_); + } + + if (cxsparse_factor_ == NULL) { + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "CXSparse failure. Unable to find symbolic factorization."; + } else if (!cxsparse_.SolveCholesky(lhs, cxsparse_factor_, solution)) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "CXSparse::SolveCholesky failed."; + } + + cxsparse_.Free(lhs); + return summary; +#endif // CERES_NO_CXPARSE +} + +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessSparseMatrix. The linear system is solved using +// Eigen's sparse cholesky factorization routines. +LinearSolver::Summary +SparseSchurComplementSolver::SolveReducedLinearSystemUsingEigen( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution) { +#ifndef CERES_USE_EIGEN_SPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "SPARSE_SCHUR cannot be used with EIGEN_SPARSE. " + "Ceres was not built with support for " + "Eigen's SimplicialLDLT decomposition. " + "This requires enabling building with -DEIGENSPARSE=ON."; + return summary; + +#else + EventLogger event_logger("SchurComplementSolver::EigenSolve"); + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + // Extract the TripletSparseMatrix that is used for actually storing S. + TripletSparseMatrix* tsm = + const_cast<TripletSparseMatrix*>( + down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); + const int num_rows = tsm->num_rows(); + + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + return summary; + } + + // This is an upper triangular matrix. + CompressedRowSparseMatrix crsm(*tsm); + // Map this to a column major, lower triangular matrix. + Eigen::MappedSparseMatrix<double, Eigen::ColMajor> eigen_lhs( + crsm.num_rows(), + crsm.num_rows(), + crsm.num_nonzeros(), + crsm.mutable_rows(), + crsm.mutable_cols(), + crsm.mutable_values()); + event_logger.AddEvent("ToCompressedRowSparseMatrix"); + + // Compute symbolic factorization if one does not exist. + if (simplicial_ldlt_.get() == NULL) { + simplicial_ldlt_.reset(new SimplicialLDLT); + // This ordering is quite bad. The scalar ordering produced by the + // AMD algorithm is quite bad and can be an order of magnitude + // worse than the one computed using the block version of the + // algorithm. + simplicial_ldlt_->analyzePattern(eigen_lhs); + event_logger.AddEvent("Analysis"); + if (simplicial_ldlt_->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "Eigen failure. Unable to find symbolic factorization."; + return summary; + } + } + + simplicial_ldlt_->factorize(eigen_lhs); + event_logger.AddEvent("Factorize"); + if (simplicial_ldlt_->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Eigen failure. Unable to find numeric factoriztion."; + return summary; + } + + VectorRef(solution, num_rows) = + simplicial_ldlt_->solve(ConstVectorRef(rhs(), num_rows)); + event_logger.AddEvent("Solve"); + if (simplicial_ldlt_->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Eigen failure. Unable to do triangular solve."; + } + + return summary; +#endif // CERES_USE_EIGEN_SPARSE +} + +LinearSolver::Summary +SparseSchurComplementSolver::SolveReducedLinearSystemUsingConjugateGradients( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution) { + const int num_rows = lhs()->num_rows(); + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + return summary; + } + + // Only SCHUR_JACOBI is supported over here right now. + CHECK_EQ(options().preconditioner_type, SCHUR_JACOBI); + + if (preconditioner_.get() == NULL) { + preconditioner_.reset(new BlockRandomAccessDiagonalMatrix(blocks_)); + } + + BlockRandomAccessSparseMatrix* sc = + down_cast<BlockRandomAccessSparseMatrix*>( + const_cast<BlockRandomAccessMatrix*>(lhs())); + + // Extract block diagonal from the Schur complement to construct the + // schur_jacobi preconditioner. + for (int i = 0; i < blocks_.size(); ++i) { + const int block_size = blocks_[i]; + + int sc_r, sc_c, sc_row_stride, sc_col_stride; + CellInfo* sc_cell_info = + CHECK_NOTNULL(sc->GetCell(i, i, + &sc_r, &sc_c, + &sc_row_stride, &sc_col_stride)); + MatrixRef sc_m(sc_cell_info->values, sc_row_stride, sc_col_stride); + + int pre_r, pre_c, pre_row_stride, pre_col_stride; + CellInfo* pre_cell_info = CHECK_NOTNULL( + preconditioner_->GetCell(i, i, + &pre_r, &pre_c, + &pre_row_stride, &pre_col_stride)); + MatrixRef pre_m(pre_cell_info->values, pre_row_stride, pre_col_stride); + + pre_m.block(pre_r, pre_c, block_size, block_size) = + sc_m.block(sc_r, sc_c, block_size, block_size); + } + preconditioner_->Invert(); + + VectorRef(solution, num_rows).setZero(); + + scoped_ptr<LinearOperator> lhs_adapter( + new BlockRandomAccessSparseMatrixAdapter(*sc)); + scoped_ptr<LinearOperator> preconditioner_adapter( + new BlockRandomAccessDiagonalMatrixAdapter(*preconditioner_)); + + + LinearSolver::Options cg_options; + cg_options.min_num_iterations = options().min_num_iterations; + cg_options.max_num_iterations = options().max_num_iterations; + ConjugateGradientsSolver cg_solver(cg_options); + + LinearSolver::PerSolveOptions cg_per_solve_options; + cg_per_solve_options.r_tolerance = per_solve_options.r_tolerance; + cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance; + cg_per_solve_options.preconditioner = preconditioner_adapter.get(); + + return cg_solver.Solve(lhs_adapter.get(), + rhs(), + cg_per_solve_options, + solution); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/schur_complement_solver.h b/extern/ceres/internal/ceres/schur_complement_solver.h new file mode 100644 index 00000000000..714dafc5b0c --- /dev/null +++ b/extern/ceres/internal/ceres/schur_complement_solver.h @@ -0,0 +1,226 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ +#define CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ + +#include <set> +#include <utility> +#include <vector> + +#include "ceres/internal/port.h" + +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/cxsparse.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/suitesparse.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" +#include "ceres/block_random_access_diagonal_matrix.h" + +#ifdef CERES_USE_EIGEN_SPARSE +#include "Eigen/SparseCholesky" +#include "Eigen/OrderingMethods" +#endif + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; + +// Base class for Schur complement based linear least squares +// solvers. It assumes that the input linear system Ax = b can be +// partitioned into +// +// E y + F z = b +// +// Where x = [y;z] is a partition of the variables. The paritioning +// of the variables is such that, E'E is a block diagonal +// matrix. Further, the rows of A are ordered so that for every +// variable block in y, all the rows containing that variable block +// occur as a vertically contiguous block. i.e the matrix A looks like +// +// E F +// A = [ y1 0 0 0 | z1 0 0 0 z5] +// [ y1 0 0 0 | z1 z2 0 0 0] +// [ 0 y2 0 0 | 0 0 z3 0 0] +// [ 0 0 y3 0 | z1 z2 z3 z4 z5] +// [ 0 0 y3 0 | z1 0 0 0 z5] +// [ 0 0 0 y4 | 0 0 0 0 z5] +// [ 0 0 0 y4 | 0 z2 0 0 0] +// [ 0 0 0 y4 | 0 0 0 0 0] +// [ 0 0 0 0 | z1 0 0 0 0] +// [ 0 0 0 0 | 0 0 z3 z4 z5] +// +// This structure should be reflected in the corresponding +// CompressedRowBlockStructure object associated with A. The linear +// system Ax = b should either be well posed or the array D below +// should be non-null and the diagonal matrix corresponding to it +// should be non-singular. +// +// SchurComplementSolver has two sub-classes. +// +// DenseSchurComplementSolver: For problems where the Schur complement +// matrix is small and dense, or if CHOLMOD/SuiteSparse is not +// installed. For structure from motion problems, this is solver can +// be used for problems with upto a few hundred cameras. +// +// SparseSchurComplementSolver: For problems where the Schur +// complement matrix is large and sparse. It requires that +// CHOLMOD/SuiteSparse be installed, as it uses CHOLMOD to find a +// sparse Cholesky factorization of the Schur complement. This solver +// can be used for solving structure from motion problems with tens of +// thousands of cameras, though depending on the exact sparsity +// structure, it maybe better to use an iterative solver. +// +// The two solvers can be instantiated by calling +// LinearSolver::CreateLinearSolver with LinearSolver::Options::type +// set to DENSE_SCHUR and SPARSE_SCHUR +// respectively. LinearSolver::Options::elimination_groups[0] should be +// at least 1. +class SchurComplementSolver : public BlockSparseMatrixSolver { + public: + explicit SchurComplementSolver(const LinearSolver::Options& options) + : options_(options) { + CHECK_GT(options.elimination_groups.size(), 1); + CHECK_GT(options.elimination_groups[0], 0); + } + + // LinearSolver methods + virtual ~SchurComplementSolver() {} + virtual LinearSolver::Summary SolveImpl( + BlockSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + protected: + const LinearSolver::Options& options() const { return options_; } + + const BlockRandomAccessMatrix* lhs() const { return lhs_.get(); } + void set_lhs(BlockRandomAccessMatrix* lhs) { lhs_.reset(lhs); } + const double* rhs() const { return rhs_.get(); } + void set_rhs(double* rhs) { rhs_.reset(rhs); } + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs) = 0; + virtual LinearSolver::Summary SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution) = 0; + + LinearSolver::Options options_; + + scoped_ptr<SchurEliminatorBase> eliminator_; + scoped_ptr<BlockRandomAccessMatrix> lhs_; + scoped_array<double> rhs_; + + CERES_DISALLOW_COPY_AND_ASSIGN(SchurComplementSolver); +}; + +// Dense Cholesky factorization based solver. +class DenseSchurComplementSolver : public SchurComplementSolver { + public: + explicit DenseSchurComplementSolver(const LinearSolver::Options& options) + : SchurComplementSolver(options) {} + virtual ~DenseSchurComplementSolver() {} + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs); + virtual LinearSolver::Summary SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution); + + CERES_DISALLOW_COPY_AND_ASSIGN(DenseSchurComplementSolver); +}; + +// Sparse Cholesky factorization based solver. +class SparseSchurComplementSolver : public SchurComplementSolver { + public: + explicit SparseSchurComplementSolver(const LinearSolver::Options& options); + virtual ~SparseSchurComplementSolver(); + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs); + virtual LinearSolver::Summary SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution); + LinearSolver::Summary SolveReducedLinearSystemUsingSuiteSparse( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution); + LinearSolver::Summary SolveReducedLinearSystemUsingCXSparse( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution); + LinearSolver::Summary SolveReducedLinearSystemUsingEigen( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution); + LinearSolver::Summary SolveReducedLinearSystemUsingConjugateGradients( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution); + + // Size of the blocks in the Schur complement. + std::vector<int> blocks_; + + SuiteSparse ss_; + // Symbolic factorization of the reduced linear system. Precomputed + // once and reused in subsequent calls. + cholmod_factor* factor_; + + CXSparse cxsparse_; + // Cached factorization + cs_dis* cxsparse_factor_; + +#ifdef CERES_USE_EIGEN_SPARSE + + // The preprocessor gymnastics here are dealing with the fact that + // before version 3.2.2, Eigen did not support a third template + // parameter to specify the ordering. +#if EIGEN_VERSION_AT_LEAST(3,2,2) + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Lower, + Eigen::NaturalOrdering<int> > + SimplicialLDLT; +#else + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Lower> + SimplicialLDLT; +#endif + + scoped_ptr<SimplicialLDLT> simplicial_ldlt_; +#endif + + scoped_ptr<BlockRandomAccessDiagonalMatrix> preconditioner_; + CERES_DISALLOW_COPY_AND_ASSIGN(SparseSchurComplementSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/schur_eliminator.cc b/extern/ceres/internal/ceres/schur_eliminator.cc new file mode 100644 index 00000000000..ec0e2a020e5 --- /dev/null +++ b/extern/ceres/internal/ceres/schur_eliminator.cc @@ -0,0 +1,163 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specialization.py. +// Editing it manually is not recommended. + +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +SchurEliminatorBase* +SchurEliminatorBase::Create(const LinearSolver::Options& options) { +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 2)) { + return new SchurEliminator<2, 2, 2>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 2, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 2, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == Eigen::Dynamic)) { + return new SchurEliminator<2, 2, Eigen::Dynamic>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 3, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 3, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 6)) { + return new SchurEliminator<2, 3, 6>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 9)) { + return new SchurEliminator<2, 3, 9>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == Eigen::Dynamic)) { + return new SchurEliminator<2, 3, Eigen::Dynamic>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 4, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 4, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 8)) { + return new SchurEliminator<2, 4, 8>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 9)) { + return new SchurEliminator<2, 4, 9>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == Eigen::Dynamic)) { + return new SchurEliminator<2, 4, Eigen::Dynamic>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == Eigen::Dynamic) && + (options.f_block_size == Eigen::Dynamic)) { + return new SchurEliminator<2, Eigen::Dynamic, Eigen::Dynamic>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 2)) { + return new SchurEliminator<4, 4, 2>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new SchurEliminator<4, 4, 3>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new SchurEliminator<4, 4, 4>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == Eigen::Dynamic)) { + return new SchurEliminator<4, 4, Eigen::Dynamic>(options); + } + if ((options.row_block_size == Eigen::Dynamic) && + (options.e_block_size == Eigen::Dynamic) && + (options.f_block_size == Eigen::Dynamic)) { + return new SchurEliminator<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>(options); + } + +#endif + VLOG(1) << "Template specializations not found for <" + << options.row_block_size << "," + << options.e_block_size << "," + << options.f_block_size << ">"; + return new SchurEliminator<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>(options); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/schur_eliminator.h b/extern/ceres/internal/ceres/schur_eliminator.h new file mode 100644 index 00000000000..761b58adc7f --- /dev/null +++ b/extern/ceres/internal/ceres/schur_eliminator.h @@ -0,0 +1,355 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_SCHUR_ELIMINATOR_H_ +#define CERES_INTERNAL_SCHUR_ELIMINATOR_H_ + +#include <map> +#include <vector> +#include "ceres/mutex.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/linear_solver.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// Classes implementing the SchurEliminatorBase interface implement +// variable elimination for linear least squares problems. Assuming +// that the input linear system Ax = b can be partitioned into +// +// E y + F z = b +// +// Where x = [y;z] is a partition of the variables. The paritioning +// of the variables is such that, E'E is a block diagonal matrix. Or +// in other words, the parameter blocks in E form an independent set +// of the of the graph implied by the block matrix A'A. Then, this +// class provides the functionality to compute the Schur complement +// system +// +// S z = r +// +// where +// +// S = F'F - F'E (E'E)^{-1} E'F and r = F'b - F'E(E'E)^(-1) E'b +// +// This is the Eliminate operation, i.e., construct the linear system +// obtained by eliminating the variables in E. +// +// The eliminator also provides the reverse functionality, i.e. given +// values for z it can back substitute for the values of y, by solving the +// linear system +// +// Ey = b - F z +// +// which is done by observing that +// +// y = (E'E)^(-1) [E'b - E'F z] +// +// The eliminator has a number of requirements. +// +// The rows of A are ordered so that for every variable block in y, +// all the rows containing that variable block occur as a vertically +// contiguous block. i.e the matrix A looks like +// +// E F chunk +// A = [ y1 0 0 0 | z1 0 0 0 z5] 1 +// [ y1 0 0 0 | z1 z2 0 0 0] 1 +// [ 0 y2 0 0 | 0 0 z3 0 0] 2 +// [ 0 0 y3 0 | z1 z2 z3 z4 z5] 3 +// [ 0 0 y3 0 | z1 0 0 0 z5] 3 +// [ 0 0 0 y4 | 0 0 0 0 z5] 4 +// [ 0 0 0 y4 | 0 z2 0 0 0] 4 +// [ 0 0 0 y4 | 0 0 0 0 0] 4 +// [ 0 0 0 0 | z1 0 0 0 0] non chunk blocks +// [ 0 0 0 0 | 0 0 z3 z4 z5] non chunk blocks +// +// This structure should be reflected in the corresponding +// CompressedRowBlockStructure object associated with A. The linear +// system Ax = b should either be well posed or the array D below +// should be non-null and the diagonal matrix corresponding to it +// should be non-singular. For simplicity of exposition only the case +// with a null D is described. +// +// The usual way to do the elimination is as follows. Starting with +// +// E y + F z = b +// +// we can form the normal equations, +// +// E'E y + E'F z = E'b +// F'E y + F'F z = F'b +// +// multiplying both sides of the first equation by (E'E)^(-1) and then +// by F'E we get +// +// F'E y + F'E (E'E)^(-1) E'F z = F'E (E'E)^(-1) E'b +// F'E y + F'F z = F'b +// +// now subtracting the two equations we get +// +// [FF' - F'E (E'E)^(-1) E'F] z = F'b - F'E(E'E)^(-1) E'b +// +// Instead of forming the normal equations and operating on them as +// general sparse matrices, the algorithm here deals with one +// parameter block in y at a time. The rows corresponding to a single +// parameter block yi are known as a chunk, and the algorithm operates +// on one chunk at a time. The mathematics remains the same since the +// reduced linear system can be shown to be the sum of the reduced +// linear systems for each chunk. This can be seen by observing two +// things. +// +// 1. E'E is a block diagonal matrix. +// +// 2. When E'F is computed, only the terms within a single chunk +// interact, i.e for y1 column blocks when transposed and multiplied +// with F, the only non-zero contribution comes from the blocks in +// chunk1. +// +// Thus, the reduced linear system +// +// FF' - F'E (E'E)^(-1) E'F +// +// can be re-written as +// +// sum_k F_k F_k' - F_k'E_k (E_k'E_k)^(-1) E_k' F_k +// +// Where the sum is over chunks and E_k'E_k is dense matrix of size y1 +// x y1. +// +// Advanced usage. Uptil now it has been assumed that the user would +// be interested in all of the Schur Complement S. However, it is also +// possible to use this eliminator to obtain an arbitrary submatrix of +// the full Schur complement. When the eliminator is generating the +// blocks of S, it asks the RandomAccessBlockMatrix instance passed to +// it if it has storage for that block. If it does, the eliminator +// computes/updates it, if not it is skipped. This is useful when one +// is interested in constructing a preconditioner based on the Schur +// Complement, e.g., computing the block diagonal of S so that it can +// be used as a preconditioner for an Iterative Substructuring based +// solver [See Agarwal et al, Bundle Adjustment in the Large, ECCV +// 2008 for an example of such use]. +// +// Example usage: Please see schur_complement_solver.cc +class SchurEliminatorBase { + public: + virtual ~SchurEliminatorBase() {} + + // Initialize the eliminator. It is the user's responsibilty to call + // this function before calling Eliminate or BackSubstitute. It is + // also the caller's responsibilty to ensure that the + // CompressedRowBlockStructure object passed to this method is the + // same one (or is equivalent to) the one associated with the + // BlockSparseMatrix objects below. + virtual void Init(int num_eliminate_blocks, + const CompressedRowBlockStructure* bs) = 0; + + // Compute the Schur complement system from the augmented linear + // least squares problem [A;D] x = [b;0]. The left hand side and the + // right hand side of the reduced linear system are returned in lhs + // and rhs respectively. + // + // It is the caller's responsibility to construct and initialize + // lhs. Depending upon the structure of the lhs object passed here, + // the full or a submatrix of the Schur complement will be computed. + // + // Since the Schur complement is a symmetric matrix, only the upper + // triangular part of the Schur complement is computed. + virtual void Eliminate(const BlockSparseMatrix* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs) = 0; + + // Given values for the variables z in the F block of A, solve for + // the optimal values of the variables y corresponding to the E + // block in A. + virtual void BackSubstitute(const BlockSparseMatrix* A, + const double* b, + const double* D, + const double* z, + double* y) = 0; + // Factory + static SchurEliminatorBase* Create(const LinearSolver::Options& options); +}; + +// Templated implementation of the SchurEliminatorBase interface. The +// templating is on the sizes of the row, e and f blocks sizes in the +// input matrix. In many problems, the sizes of one or more of these +// blocks are constant, in that case, its worth passing these +// parameters as template arguments so that they are visible to the +// compiler and can be used for compile time optimization of the low +// level linear algebra routines. +// +// This implementation is mulithreaded using OpenMP. The level of +// parallelism is controlled by LinearSolver::Options::num_threads. +template <int kRowBlockSize = Eigen::Dynamic, + int kEBlockSize = Eigen::Dynamic, + int kFBlockSize = Eigen::Dynamic > +class SchurEliminator : public SchurEliminatorBase { + public: + explicit SchurEliminator(const LinearSolver::Options& options) + : num_threads_(options.num_threads) { + } + + // SchurEliminatorBase Interface + virtual ~SchurEliminator(); + virtual void Init(int num_eliminate_blocks, + const CompressedRowBlockStructure* bs); + virtual void Eliminate(const BlockSparseMatrix* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs); + virtual void BackSubstitute(const BlockSparseMatrix* A, + const double* b, + const double* D, + const double* z, + double* y); + + private: + // Chunk objects store combinatorial information needed to + // efficiently eliminate a whole chunk out of the least squares + // problem. Consider the first chunk in the example matrix above. + // + // [ y1 0 0 0 | z1 0 0 0 z5] + // [ y1 0 0 0 | z1 z2 0 0 0] + // + // One of the intermediate quantities that needs to be calculated is + // for each row the product of the y block transposed with the + // non-zero z block, and the sum of these blocks across rows. A + // temporary array "buffer_" is used for computing and storing them + // and the buffer_layout maps the indices of the z-blocks to + // position in the buffer_ array. The size of the chunk is the + // number of row blocks/residual blocks for the particular y block + // being considered. + // + // For the example chunk shown above, + // + // size = 2 + // + // The entries of buffer_layout will be filled in the following order. + // + // buffer_layout[z1] = 0 + // buffer_layout[z5] = y1 * z1 + // buffer_layout[z2] = y1 * z1 + y1 * z5 + typedef std::map<int, int> BufferLayoutType; + struct Chunk { + Chunk() : size(0) {} + int size; + int start; + BufferLayoutType buffer_layout; + }; + + void ChunkDiagonalBlockAndGradient( + const Chunk& chunk, + const BlockSparseMatrix* A, + const double* b, + int row_block_counter, + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* eet, + double* g, + double* buffer, + BlockRandomAccessMatrix* lhs); + + void UpdateRhs(const Chunk& chunk, + const BlockSparseMatrix* A, + const double* b, + int row_block_counter, + const double* inverse_ete_g, + double* rhs); + + void ChunkOuterProduct(const CompressedRowBlockStructure* bs, + const Matrix& inverse_eet, + const double* buffer, + const BufferLayoutType& buffer_layout, + BlockRandomAccessMatrix* lhs); + void EBlockRowOuterProduct(const BlockSparseMatrix* A, + int row_block_index, + BlockRandomAccessMatrix* lhs); + + + void NoEBlockRowsUpdate(const BlockSparseMatrix* A, + const double* b, + int row_block_counter, + BlockRandomAccessMatrix* lhs, + double* rhs); + + void NoEBlockRowOuterProduct(const BlockSparseMatrix* A, + int row_block_index, + BlockRandomAccessMatrix* lhs); + + int num_eliminate_blocks_; + + // Block layout of the columns of the reduced linear system. Since + // the f blocks can be of varying size, this vector stores the + // position of each f block in the row/col of the reduced linear + // system. Thus lhs_row_layout_[i] is the row/col position of the + // i^th f block. + std::vector<int> lhs_row_layout_; + + // Combinatorial structure of the chunks in A. For more information + // see the documentation of the Chunk object above. + std::vector<Chunk> chunks_; + + // TODO(sameeragarwal): The following two arrays contain per-thread + // storage. They should be refactored into a per thread struct. + + // Buffer to store the products of the y and z blocks generated + // during the elimination phase. buffer_ is of size num_threads * + // buffer_size_. Each thread accesses the chunk + // + // [thread_id * buffer_size_ , (thread_id + 1) * buffer_size_] + // + scoped_array<double> buffer_; + + // Buffer to store per thread matrix matrix products used by + // ChunkOuterProduct. Like buffer_ it is of size num_threads * + // buffer_size_. Each thread accesses the chunk + // + // [thread_id * buffer_size_ , (thread_id + 1) * buffer_size_ -1] + // + scoped_array<double> chunk_outer_product_buffer_; + + int buffer_size_; + int num_threads_; + int uneliminated_row_begins_; + + // Locks for the blocks in the right hand side of the reduced linear + // system. + std::vector<Mutex*> rhs_locks_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_ELIMINATOR_H_ diff --git a/extern/ceres/internal/ceres/schur_eliminator_impl.h b/extern/ceres/internal/ceres/schur_eliminator_impl.h new file mode 100644 index 00000000000..f2535880f15 --- /dev/null +++ b/extern/ceres/internal/ceres/schur_eliminator_impl.h @@ -0,0 +1,698 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// TODO(sameeragarwal): row_block_counter can perhaps be replaced by +// Chunk::start ? + +#ifndef CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ +#define CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ + +// Eigen has an internal threshold switching between different matrix +// multiplication algorithms. In particular for matrices larger than +// EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD it uses a cache friendly +// matrix matrix product algorithm that has a higher setup cost. For +// matrix sizes close to this threshold, especially when the matrices +// are thin and long, the default choice may not be optimal. This is +// the case for us, as the default choice causes a 30% performance +// regression when we moved from Eigen2 to Eigen3. + +#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10 + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +#include <algorithm> +#include <map> +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#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" + +namespace ceres { +namespace internal { + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::~SchurEliminator() { + STLDeleteElements(&rhs_locks_); +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) { + CHECK_GT(num_eliminate_blocks, 0) + << "SchurComplementSolver cannot be initialized with " + << "num_eliminate_blocks = 0."; + + num_eliminate_blocks_ = num_eliminate_blocks; + + const int num_col_blocks = bs->cols.size(); + const int num_row_blocks = bs->rows.size(); + + buffer_size_ = 1; + chunks_.clear(); + lhs_row_layout_.clear(); + + int lhs_num_rows = 0; + // Add a map object for each block in the reduced linear system + // and build the row/column block structure of the reduced linear + // system. + lhs_row_layout_.resize(num_col_blocks - num_eliminate_blocks_); + for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { + lhs_row_layout_[i - num_eliminate_blocks_] = lhs_num_rows; + lhs_num_rows += bs->cols[i].size; + } + + int r = 0; + // Iterate over the row blocks of A, and detect the chunks. The + // matrix should already have been ordered so that all rows + // containing the same y block are vertically contiguous. Along + // the way also compute the amount of space each chunk will need + // to perform the elimination. + while (r < num_row_blocks) { + const int chunk_block_id = bs->rows[r].cells.front().block_id; + if (chunk_block_id >= num_eliminate_blocks_) { + break; + } + + chunks_.push_back(Chunk()); + Chunk& chunk = chunks_.back(); + chunk.size = 0; + chunk.start = r; + int buffer_size = 0; + const int e_block_size = bs->cols[chunk_block_id].size; + + // Add to the chunk until the first block in the row is + // different than the one in the first row for the chunk. + while (r + chunk.size < num_row_blocks) { + const CompressedRow& row = bs->rows[r + chunk.size]; + if (row.cells.front().block_id != chunk_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first + // block since it is the one to be eliminated. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + if (InsertIfNotPresent( + &(chunk.buffer_layout), cell.block_id, buffer_size)) { + buffer_size += e_block_size * bs->cols[cell.block_id].size; + } + } + + buffer_size_ = std::max(buffer_size, buffer_size_); + ++chunk.size; + } + + CHECK_GT(chunk.size, 0); + r += chunk.size; + } + const Chunk& chunk = chunks_.back(); + + uneliminated_row_begins_ = chunk.start + chunk.size; + if (num_threads_ > 1) { + random_shuffle(chunks_.begin(), chunks_.end()); + } + + buffer_.reset(new double[buffer_size_ * num_threads_]); + + // chunk_outer_product_buffer_ only needs to store e_block_size * + // f_block_size, which is always less than buffer_size_, so we just + // allocate buffer_size_ per thread. + chunk_outer_product_buffer_.reset(new double[buffer_size_ * num_threads_]); + + STLDeleteElements(&rhs_locks_); + rhs_locks_.resize(num_col_blocks - num_eliminate_blocks_); + for (int i = 0; i < num_col_blocks - num_eliminate_blocks_; ++i) { + rhs_locks_[i] = new Mutex; + } +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +Eliminate(const BlockSparseMatrix* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs) { + if (lhs->num_rows() > 0) { + lhs->SetZero(); + VectorRef(rhs, lhs->num_rows()).setZero(); + } + + const CompressedRowBlockStructure* bs = A->block_structure(); + const int num_col_blocks = bs->cols.size(); + + // Add the diagonal to the schur complement. + if (D != NULL) { +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { + const int block_id = i - num_eliminate_blocks_; + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block_id, block_id, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + const int block_size = bs->cols[i].size; + typename EigenTypes<Eigen::Dynamic>::ConstVectorRef + diag(D + bs->cols[i].position, block_size); + + CeresMutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size, block_size).diagonal() + += diag.array().square().matrix(); + } + } + } + + // Eliminate y blocks one chunk at a time. For each chunk, compute + // the entries of the normal equations and the gradient vector block + // corresponding to the y block and then apply Gaussian elimination + // to them. The matrix ete stores the normal matrix corresponding to + // the block being eliminated and array buffer_ contains the + // non-zero blocks in the row corresponding to this y block in the + // normal equations. This computation is done in + // ChunkDiagonalBlockAndGradient. UpdateRhs then applies gaussian + // elimination to the rhs of the normal equations, updating the rhs + // of the reduced linear system by modifying rhs blocks for all the + // z blocks that share a row block/residual term with the y + // block. EliminateRowOuterProduct does the corresponding operation + // for the lhs of the reduced linear system. +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = 0; i < chunks_.size(); ++i) { +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + double* buffer = buffer_.get() + thread_id * buffer_size_; + const Chunk& chunk = chunks_[i]; + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + const int e_block_size = bs->cols[e_block_id].size; + + VectorRef(buffer, buffer_size_).setZero(); + + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix + ete(e_block_size, e_block_size); + + if (D != NULL) { + const typename EigenTypes<kEBlockSize>::ConstVectorRef + diag(D + bs->cols[e_block_id].position, e_block_size); + ete = diag.array().square().matrix().asDiagonal(); + } else { + ete.setZero(); + } + + FixedArray<double, 8> g(e_block_size); + typename EigenTypes<kEBlockSize>::VectorRef gref(g.get(), e_block_size); + gref.setZero(); + + // We are going to be computing + // + // S += F'F - F'E(E'E)^{-1}E'F + // + // for each Chunk. The computation is broken down into a number of + // function calls as below. + + // Compute the outer product of the e_blocks with themselves (ete + // = E'E). Compute the product of the e_blocks with the + // corresonding f_blocks (buffer = E'F), the gradient of the terms + // in this chunk (g) and add the outer product of the f_blocks to + // Schur complement (S += F'F). + ChunkDiagonalBlockAndGradient( + chunk, A, b, chunk.start, &ete, g.get(), buffer, lhs); + + // Normally one wouldn't compute the inverse explicitly, but + // e_block_size will typically be a small number like 3, in + // which case its much faster to compute the inverse once and + // use it to multiply other matrices/vectors instead of doing a + // Solve call over and over again. + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix inverse_ete = + ete + .template selfadjointView<Eigen::Upper>() + .llt() + .solve(Matrix::Identity(e_block_size, e_block_size)); + + // For the current chunk compute and update the rhs of the reduced + // linear system. + // + // rhs = F'b - F'E(E'E)^(-1) E'b + + FixedArray<double, 8> inverse_ete_g(e_block_size); + MatrixVectorMultiply<kEBlockSize, kEBlockSize, 0>( + inverse_ete.data(), + e_block_size, + e_block_size, + g.get(), + inverse_ete_g.get()); + + UpdateRhs(chunk, A, b, chunk.start, inverse_ete_g.get(), rhs); + + // S -= F'E(E'E)^{-1}E'F + ChunkOuterProduct(bs, inverse_ete, buffer, chunk.buffer_layout, lhs); + } + + // For rows with no e_blocks, the schur complement update reduces to + // S += F'F. + NoEBlockRowsUpdate(A, b, uneliminated_row_begins_, lhs, rhs); +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +BackSubstitute(const BlockSparseMatrix* A, + const double* b, + const double* D, + const double* z, + double* y) { + const CompressedRowBlockStructure* bs = A->block_structure(); +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = 0; i < chunks_.size(); ++i) { + const Chunk& chunk = chunks_[i]; + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + const int e_block_size = bs->cols[e_block_id].size; + + double* y_ptr = y + bs->cols[e_block_id].position; + typename EigenTypes<kEBlockSize>::VectorRef y_block(y_ptr, e_block_size); + + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix + ete(e_block_size, e_block_size); + if (D != NULL) { + const typename EigenTypes<kEBlockSize>::ConstVectorRef + diag(D + bs->cols[e_block_id].position, e_block_size); + ete = diag.array().square().matrix().asDiagonal(); + } else { + ete.setZero(); + } + + const double* values = A->values(); + for (int j = 0; j < chunk.size; ++j) { + const CompressedRow& row = bs->rows[chunk.start + j]; + const Cell& e_cell = row.cells.front(); + DCHECK_EQ(e_block_id, e_cell.block_id); + + FixedArray<double, 8> sj(row.block.size); + + typename EigenTypes<kRowBlockSize>::VectorRef(sj.get(), row.block.size) = + typename EigenTypes<kRowBlockSize>::ConstVectorRef + (b + bs->rows[chunk.start + j].block.position, row.block.size); + + for (int c = 1; c < row.cells.size(); ++c) { + const int f_block_id = row.cells[c].block_id; + const int f_block_size = bs->cols[f_block_id].size; + const int r_block = f_block_id - num_eliminate_blocks_; + + MatrixVectorMultiply<kRowBlockSize, kFBlockSize, -1>( + values + row.cells[c].position, row.block.size, f_block_size, + z + lhs_row_layout_[r_block], + sj.get()); + } + + MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>( + values + e_cell.position, row.block.size, e_block_size, + sj.get(), + y_ptr); + + MatrixTransposeMatrixMultiply + <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>( + 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); + } + + ete.llt().solveInPlace(y_block); + } +} + +// Update the rhs of the reduced linear system. Compute +// +// F'b - F'E(E'E)^(-1) E'b +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +UpdateRhs(const Chunk& chunk, + const BlockSparseMatrix* A, + const double* b, + int row_block_counter, + const double* inverse_ete_g, + double* rhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + 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 Cell& e_cell = row.cells.front(); + + typename EigenTypes<kRowBlockSize>::Vector sj = + typename EigenTypes<kRowBlockSize>::ConstVectorRef + (b + b_pos, row.block.size); + + MatrixVectorMultiply<kRowBlockSize, kEBlockSize, -1>( + values + e_cell.position, row.block.size, e_block_size, + inverse_ete_g, sj.data()); + + for (int c = 1; c < row.cells.size(); ++c) { + const int block_id = row.cells[c].block_id; + const int block_size = bs->cols[block_id].size; + const int block = block_id - num_eliminate_blocks_; + CeresMutexLock l(rhs_locks_[block]); + MatrixTransposeVectorMultiply<kRowBlockSize, kFBlockSize, 1>( + values + row.cells[c].position, + row.block.size, block_size, + sj.data(), rhs + lhs_row_layout_[block]); + } + b_pos += row.block.size; + } +} + +// Given a Chunk - set of rows with the same e_block, e.g. in the +// following Chunk with two rows. +// +// E F +// [ y11 0 0 0 | z11 0 0 0 z51] +// [ y12 0 0 0 | z12 z22 0 0 0] +// +// this function computes twp matrices. The diagonal block matrix +// +// ete = y11 * y11' + y12 * y12' +// +// and the off diagonal blocks in the Guass Newton Hessian. +// +// buffer = [y11'(z11 + z12), y12' * z22, y11' * z51] +// +// which are zero compressed versions of the block sparse matrices E'E +// and E'F. +// +// and the gradient of the e_block, E'b. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +ChunkDiagonalBlockAndGradient( + const Chunk& chunk, + const BlockSparseMatrix* A, + const double* b, + int row_block_counter, + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* ete, + double* g, + double* buffer, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + + int b_pos = bs->rows[row_block_counter].block.position; + const int e_block_size = ete->rows(); + + // Iterate over the rows in this chunk, for each row, compute the + // contribution of its F blocks to the Schur complement, the + // contribution of its E block to the matrix EE' (ete), and the + // corresponding block in the gradient vector. + const double* values = A->values(); + for (int j = 0; j < chunk.size; ++j) { + const CompressedRow& row = bs->rows[row_block_counter + j]; + + if (row.cells.size() > 1) { + EBlockRowOuterProduct(A, row_block_counter + j, lhs); + } + + // Extract the e_block, ETE += E_i' E_i + const Cell& e_cell = row.cells.front(); + MatrixTransposeMatrixMultiply + <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>( + 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>( + values + e_cell.position, row.block.size, e_block_size, + b + b_pos, + g); + + + // buffer = E'F. This computation is done by iterating over the + // f_blocks for each row in the chunk. + for (int c = 1; c < row.cells.size(); ++c) { + const int f_block_id = row.cells[c].block_id; + const int f_block_size = bs->cols[f_block_id].size; + double* buffer_ptr = + buffer + FindOrDie(chunk.buffer_layout, f_block_id); + MatrixTransposeMatrixMultiply + <kRowBlockSize, kEBlockSize, kRowBlockSize, kFBlockSize, 1>( + 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; + } +} + +// Compute the outer product F'E(E'E)^{-1}E'F and subtract it from the +// Schur complement matrix, i.e +// +// S -= F'E(E'E)^{-1}E'F. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +ChunkOuterProduct(const CompressedRowBlockStructure* bs, + const Matrix& inverse_ete, + const double* buffer, + const BufferLayoutType& buffer_layout, + BlockRandomAccessMatrix* lhs) { + // This is the most computationally expensive part of this + // code. Profiling experiments reveal that the bottleneck is not the + // computation of the right-hand matrix product, but memory + // references to the left hand side. + const int e_block_size = inverse_ete.rows(); + BufferLayoutType::const_iterator it1 = buffer_layout.begin(); + +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + double* b1_transpose_inverse_ete = + chunk_outer_product_buffer_.get() + thread_id * buffer_size_; + + // S(i,j) -= bi' * ete^{-1} b_j + for (; it1 != buffer_layout.end(); ++it1) { + const int block1 = it1->first - num_eliminate_blocks_; + const int block1_size = bs->cols[it1->first].size; + MatrixTransposeMatrixMultiply + <kEBlockSize, kFBlockSize, kEBlockSize, kEBlockSize, 0>( + buffer + it1->second, e_block_size, block1_size, + inverse_ete.data(), e_block_size, e_block_size, + b1_transpose_inverse_ete, 0, 0, block1_size, e_block_size); + + BufferLayoutType::const_iterator it2 = it1; + for (; it2 != buffer_layout.end(); ++it2) { + const int block2 = it2->first - num_eliminate_blocks_; + + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + const int block2_size = bs->cols[it2->first].size; + CeresMutexLock l(&cell_info->m); + MatrixMatrixMultiply + <kFBlockSize, kEBlockSize, kEBlockSize, kFBlockSize, -1>( + b1_transpose_inverse_ete, block1_size, e_block_size, + buffer + it2->second, e_block_size, block2_size, + cell_info->values, r, c, row_stride, col_stride); + } + } + } +} + +// For rows with no e_blocks, the schur complement update reduces to S +// += F'F. This function iterates over the rows of A with no e_block, +// and calls NoEBlockRowOuterProduct on each row. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +NoEBlockRowsUpdate(const 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]; + 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>( + values + row.cells[c].position, row.block.size, block_size, + b + row.block.position, + rhs + lhs_row_layout_[block]); + } + NoEBlockRowOuterProduct(A, row_block_counter, lhs); + } +} + + +// A row r of A, which has no e_blocks gets added to the Schur +// Complement as S += r r'. This function is responsible for computing +// the contribution of a single row r to the Schur complement. It is +// very similar in structure to EBlockRowOuterProduct except for +// one difference. It does not use any of the template +// parameters. This is because the algorithm used for detecting the +// static structure of the matrix A only pays attention to rows with +// e_blocks. This is becase rows without e_blocks are rare and +// typically arise from regularization terms in the original +// optimization problem, and have a very different structure than the +// rows with e_blocks. Including them in the static structure +// detection will lead to most template parameters being set to +// dynamic. Since the number of rows without e_blocks is small, the +// lack of templating is not an issue. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +NoEBlockRowOuterProduct(const BlockSparseMatrix* A, + int row_block_index, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRow& row = bs->rows[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); + + const int block1_size = bs->cols[row.cells[i].block_id].size; + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block1, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + CeresMutexLock l(&cell_info->m); + // This multiply currently ignores the fact that this is a + // symmetric outer product. + MatrixTransposeMatrixMultiply + <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( + 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); + } + + for (int j = i + 1; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks_; + DCHECK_GE(block2, 0); + DCHECK_LT(block1, block2); + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + const int block2_size = bs->cols[row.cells[j].block_id].size; + CeresMutexLock l(&cell_info->m); + MatrixTransposeMatrixMultiply + <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( + 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); + } + } + } +} + +// For a row with an e_block, compute the contribition S += F'F. This +// function has the same structure as NoEBlockRowOuterProduct, except +// that this function uses the template parameters. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +EBlockRowOuterProduct(const BlockSparseMatrix* A, + int row_block_index, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRow& row = bs->rows[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); + + const int block1_size = bs->cols[row.cells[i].block_id].size; + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block1, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + CeresMutexLock l(&cell_info->m); + // block += b1.transpose() * b1; + MatrixTransposeMatrixMultiply + <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>( + 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); + } + + for (int j = i + 1; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks_; + DCHECK_GE(block2, 0); + DCHECK_LT(block1, block2); + const int block2_size = bs->cols[row.cells[j].block_id].size; + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + // block += b1.transpose() * b2; + CeresMutexLock l(&cell_info->m); + MatrixTransposeMatrixMultiply + <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>( + 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); + } + } + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ diff --git a/extern/ceres/internal/ceres/schur_jacobi_preconditioner.cc b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.cc new file mode 100644 index 00000000000..3e6cc90f63c --- /dev/null +++ b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.cc @@ -0,0 +1,115 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/schur_jacobi_preconditioner.h" + +#include <utility> +#include <vector> +#include "ceres/block_random_access_diagonal_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/collections_port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +SchurJacobiPreconditioner::SchurJacobiPreconditioner( + const CompressedRowBlockStructure& bs, + const Preconditioner::Options& options) + : options_(options) { + CHECK_GT(options_.elimination_groups.size(), 1); + CHECK_GT(options_.elimination_groups[0], 0); + const int num_blocks = bs.cols.size() - options_.elimination_groups[0]; + CHECK_GT(num_blocks, 0) + << "Jacobian should have atleast 1 f_block for " + << "SCHUR_JACOBI preconditioner."; + + std::vector<int> blocks(num_blocks); + for (int i = 0; i < num_blocks; ++i) { + blocks[i] = bs.cols[i + options_.elimination_groups[0]].size; + } + + m_.reset(new BlockRandomAccessDiagonalMatrix(blocks)); + InitEliminator(bs); +} + +SchurJacobiPreconditioner::~SchurJacobiPreconditioner() { +} + +// Initialize the SchurEliminator. +void SchurJacobiPreconditioner::InitEliminator( + const CompressedRowBlockStructure& bs) { + LinearSolver::Options eliminator_options; + eliminator_options.elimination_groups = options_.elimination_groups; + eliminator_options.num_threads = options_.num_threads; + eliminator_options.e_block_size = options_.e_block_size; + eliminator_options.f_block_size = options_.f_block_size; + eliminator_options.row_block_size = options_.row_block_size; + eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); + eliminator_->Init(eliminator_options.elimination_groups[0], &bs); +} + +// Update the values of the preconditioner matrix and factorize it. +bool SchurJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, + const double* D) { + const int num_rows = m_->num_rows(); + CHECK_GT(num_rows, 0); + + // We need a dummy rhs vector and a dummy b vector since the Schur + // eliminator combines the computation of the reduced camera matrix + // with the computation of the right hand side of that linear + // system. + // + // TODO(sameeragarwal): Perhaps its worth refactoring the + // SchurEliminator::Eliminate function to allow NULL for the rhs. As + // of now it does not seem to be worth the effort. + Vector rhs = Vector::Zero(m_->num_rows()); + Vector b = Vector::Zero(A.num_rows()); + + // Compute a subset of the entries of the Schur complement. + eliminator_->Eliminate(&A, b.data(), D, m_.get(), rhs.data()); + m_->Invert(); + return true; +} + +void SchurJacobiPreconditioner::RightMultiply(const double* x, + double* y) const { + m_->RightMultiply(x, y); +} + +int SchurJacobiPreconditioner::num_rows() const { + return m_->num_rows(); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/schur_jacobi_preconditioner.h b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.h new file mode 100644 index 00000000000..5398f3ff35d --- /dev/null +++ b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.h @@ -0,0 +1,106 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// Detailed descriptions of these preconditions beyond what is +// documented here can be found in +// +// Bundle Adjustment in the Large +// S. Agarwal, N. Snavely, S. Seitz & R. Szeliski, ECCV 2010 +// http://www.cs.washington.edu/homes/sagarwal/bal.pdf + +#ifndef CERES_INTERNAL_SCHUR_JACOBI_PRECONDITIONER_H_ +#define CERES_INTERNAL_SCHUR_JACOBI_PRECONDITIONER_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/collections_port.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/preconditioner.h" + +namespace ceres { +namespace internal { + +class BlockRandomAccessDiagonalMatrix; +class BlockSparseMatrix; +struct CompressedRowBlockStructure; +class SchurEliminatorBase; + +// This class implements the SCHUR_JACOBI preconditioner for Structure +// from Motion/Bundle Adjustment problems. Full mathematical details +// can be found in +// +// Bundle Adjustment in the Large +// S. Agarwal, N. Snavely, S. Seitz & R. Szeliski, ECCV 2010 +// http://www.cs.washington.edu/homes/sagarwal/bal.pdf +// +// Example usage: +// +// Preconditioner::Options options; +// options.preconditioner_type = SCHUR_JACOBI; +// options.elimination_groups.push_back(num_points); +// options.elimination_groups.push_back(num_cameras); +// SchurJacobiPreconditioner preconditioner( +// *A.block_structure(), options); +// preconditioner.Update(A, NULL); +// preconditioner.RightMultiply(x, y); +// +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 + // to determine the sparsity structure of the preconditioner matrix. + // + // It has the same structural requirement as other Schur complement + // based solvers. Please see schur_eliminator.h for more details. + SchurJacobiPreconditioner(const CompressedRowBlockStructure& bs, + const Preconditioner::Options& options); + virtual ~SchurJacobiPreconditioner(); + + // Preconditioner interface. + 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_; + scoped_ptr<SchurEliminatorBase> eliminator_; + // Preconditioner matrix. + scoped_ptr<BlockRandomAccessDiagonalMatrix> m_; + CERES_DISALLOW_COPY_AND_ASSIGN(SchurJacobiPreconditioner); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_JACOBI_PRECONDITIONER_H_ diff --git a/extern/ceres/internal/ceres/scratch_evaluate_preparer.cc b/extern/ceres/internal/ceres/scratch_evaluate_preparer.cc new file mode 100644 index 00000000000..f01ef11c26f --- /dev/null +++ b/extern/ceres/internal/ceres/scratch_evaluate_preparer.cc @@ -0,0 +1,78 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/scratch_evaluate_preparer.h" + +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" + +namespace ceres { +namespace internal { + +ScratchEvaluatePreparer* ScratchEvaluatePreparer::Create( + const Program &program, + int num_threads) { + ScratchEvaluatePreparer* preparers = new ScratchEvaluatePreparer[num_threads]; + int max_derivatives_per_residual_block = + program.MaxDerivativesPerResidualBlock(); + for (int i = 0; i < num_threads; i++) { + preparers[i].Init(max_derivatives_per_residual_block); + } + return preparers; +} + +void ScratchEvaluatePreparer::Init(int max_derivatives_per_residual_block) { + jacobian_scratch_.reset( + new double[max_derivatives_per_residual_block]); +} + +// Point the jacobian blocks into the scratch area of this evaluate preparer. +void ScratchEvaluatePreparer::Prepare(const ResidualBlock* residual_block, + int /* residual_block_index */, + SparseMatrix* /* jacobian */, + double** jacobians) { + double* jacobian_block_cursor = jacobian_scratch_.get(); + int num_residuals = residual_block->NumResiduals(); + int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (parameter_block->IsConstant()) { + jacobians[j] = NULL; + } else { + jacobians[j] = jacobian_block_cursor; + jacobian_block_cursor += num_residuals * parameter_block->LocalSize(); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/scratch_evaluate_preparer.h b/extern/ceres/internal/ceres/scratch_evaluate_preparer.h new file mode 100644 index 00000000000..fa9ebd0e50e --- /dev/null +++ b/extern/ceres/internal/ceres/scratch_evaluate_preparer.h @@ -0,0 +1,69 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A scratch evaluate preparer provides temporary storage for the jacobians that +// are created when running user-provided cost functions. The evaluator takes +// care to avoid evaluating the jacobian for fixed parameters. + +#ifndef CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ +#define CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ + +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +class Program; +class ResidualBlock; +class SparseMatrix; + +class ScratchEvaluatePreparer { + public: + // Create num_threads ScratchEvaluatePreparers. + static ScratchEvaluatePreparer* Create(const Program &program, + int num_threads); + + // EvaluatePreparer interface + void Init(int max_derivatives_per_residual_block); + void Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians); + + private: + // Scratch space for the jacobians; each jacobian is packed one after another. + // There is enough scratch to hold all the jacobians for the largest residual. + scoped_array<double> jacobian_scratch_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ diff --git a/extern/ceres/internal/ceres/small_blas.h b/extern/ceres/internal/ceres/small_blas.h new file mode 100644 index 00000000000..264ac53047d --- /dev/null +++ b/extern/ceres/internal/ceres/small_blas.h @@ -0,0 +1,381 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/port.h" +#include "ceres/internal/eigen.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// 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 = (kRowB != 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.noalias() += Aref * Bref; + } else if (kOperation < 0) { + block.noalias() -= Aref * Bref; + } else { + block.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.noalias() += Aref.transpose() * Bref; + } else if (kOperation < 0) { + block.noalias() -= Aref.transpose() * Bref; + } else { + block.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_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/ceres/internal/ceres/solver.cc b/extern/ceres/internal/ceres/solver.cc new file mode 100644 index 00000000000..9f3228bb0be --- /dev/null +++ b/extern/ceres/internal/ceres/solver.cc @@ -0,0 +1,841 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/solver.h" + +#include <algorithm> +#include <sstream> // NOLINT +#include <vector> +#include "ceres/gradient_checking_cost_function.h" +#include "ceres/internal/port.h" +#include "ceres/parameter_block_ordering.h" +#include "ceres/preprocessor.h" +#include "ceres/problem.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/solver_utils.h" +#include "ceres/stringprintf.h" +#include "ceres/types.h" +#include "ceres/wall_time.h" + +namespace ceres { +namespace { + +using std::map; +using std::string; +using std::vector; + +#define OPTION_OP(x, y, OP) \ + if (!(options.x OP y)) { \ + std::stringstream ss; \ + ss << "Invalid configuration. "; \ + ss << string("Solver::Options::" #x " = ") << options.x << ". "; \ + ss << "Violated constraint: "; \ + ss << string("Solver::Options::" #x " " #OP " "#y); \ + *error = ss.str(); \ + return false; \ + } + +#define OPTION_OP_OPTION(x, y, OP) \ + if (!(options.x OP options.y)) { \ + std::stringstream ss; \ + ss << "Invalid configuration. "; \ + ss << string("Solver::Options::" #x " = ") << options.x << ". "; \ + ss << string("Solver::Options::" #y " = ") << options.y << ". "; \ + ss << "Violated constraint: "; \ + ss << string("Solver::Options::" #x); \ + ss << string(#OP " Solver::Options::" #y "."); \ + *error = ss.str(); \ + return false; \ + } + +#define OPTION_GE(x, y) OPTION_OP(x, y, >=); +#define OPTION_GT(x, y) OPTION_OP(x, y, >); +#define OPTION_LE(x, y) OPTION_OP(x, y, <=); +#define OPTION_LT(x, y) OPTION_OP(x, y, <); +#define OPTION_LE_OPTION(x, y) OPTION_OP_OPTION(x, y, <=) +#define OPTION_LT_OPTION(x, y) OPTION_OP_OPTION(x, y, <) + +bool CommonOptionsAreValid(const Solver::Options& options, string* error) { + OPTION_GE(max_num_iterations, 0); + OPTION_GE(max_solver_time_in_seconds, 0.0); + OPTION_GE(function_tolerance, 0.0); + OPTION_GE(gradient_tolerance, 0.0); + OPTION_GE(parameter_tolerance, 0.0); + OPTION_GT(num_threads, 0); + OPTION_GT(num_linear_solver_threads, 0); + if (options.check_gradients) { + OPTION_GT(gradient_check_relative_precision, 0.0); + OPTION_GT(numeric_derivative_relative_step_size, 0.0); + } + return true; +} + +bool TrustRegionOptionsAreValid(const Solver::Options& options, string* error) { + OPTION_GT(initial_trust_region_radius, 0.0); + OPTION_GT(min_trust_region_radius, 0.0); + OPTION_GT(max_trust_region_radius, 0.0); + OPTION_LE_OPTION(min_trust_region_radius, max_trust_region_radius); + OPTION_LE_OPTION(min_trust_region_radius, initial_trust_region_radius); + OPTION_LE_OPTION(initial_trust_region_radius, max_trust_region_radius); + OPTION_GE(min_relative_decrease, 0.0); + OPTION_GE(min_lm_diagonal, 0.0); + OPTION_GE(max_lm_diagonal, 0.0); + OPTION_LE_OPTION(min_lm_diagonal, max_lm_diagonal); + OPTION_GE(max_num_consecutive_invalid_steps, 0); + OPTION_GT(eta, 0.0); + OPTION_GE(min_linear_solver_iterations, 0); + OPTION_GE(max_linear_solver_iterations, 1); + OPTION_LE_OPTION(min_linear_solver_iterations, max_linear_solver_iterations); + + if (options.use_inner_iterations) { + OPTION_GE(inner_iteration_tolerance, 0.0); + } + + if (options.use_nonmonotonic_steps) { + OPTION_GT(max_consecutive_nonmonotonic_steps, 0); + } + + if (options.linear_solver_type == ITERATIVE_SCHUR && + options.use_explicit_schur_complement && + options.preconditioner_type != SCHUR_JACOBI) { + *error = "use_explicit_schur_complement only supports " + "SCHUR_JACOBI as the preconditioner."; + return false; + } + + if (options.preconditioner_type == CLUSTER_JACOBI && + options.sparse_linear_algebra_library_type != SUITE_SPARSE) { + *error = "CLUSTER_JACOBI requires " + "Solver::Options::sparse_linear_algebra_library_type to be " + "SUITE_SPARSE"; + return false; + } + + if (options.preconditioner_type == CLUSTER_TRIDIAGONAL && + options.sparse_linear_algebra_library_type != SUITE_SPARSE) { + *error = "CLUSTER_TRIDIAGONAL requires " + "Solver::Options::sparse_linear_algebra_library_type to be " + "SUITE_SPARSE"; + return false; + } + +#ifdef CERES_NO_LAPACK + if (options.dense_linear_algebra_library_type == LAPACK) { + if (options.linear_solver_type == DENSE_NORMAL_CHOLESKY) { + *error = "Can't use DENSE_NORMAL_CHOLESKY with LAPACK because " + "LAPACK was not enabled when Ceres was built."; + return false; + } else if (options.linear_solver_type == DENSE_QR) { + *error = "Can't use DENSE_QR with LAPACK because " + "LAPACK was not enabled when Ceres was built."; + return false; + } else if (options.linear_solver_type == DENSE_SCHUR) { + *error = "Can't use DENSE_SCHUR with LAPACK because " + "LAPACK was not enabled when Ceres was built."; + return false; + } + } +#endif + +#ifdef CERES_NO_SUITESPARSE + if (options.sparse_linear_algebra_library_type == SUITE_SPARSE) { + if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITESPARSE because " + "SuiteSparse was not enabled when Ceres was built."; + return false; + } else if (options.linear_solver_type == SPARSE_SCHUR) { + *error = "Can't use SPARSE_SCHUR with SUITESPARSE because " + "SuiteSparse was not enabled when Ceres was built."; + return false; + } else if (options.preconditioner_type == CLUSTER_JACOBI) { + *error = "CLUSTER_JACOBI preconditioner not supported. " + "SuiteSparse was not enabled when Ceres was built."; + return false; + } else if (options.preconditioner_type == CLUSTER_TRIDIAGONAL) { + *error = "CLUSTER_TRIDIAGONAL preconditioner not supported. " + "SuiteSparse was not enabled when Ceres was built."; + return false; + } + } +#endif + +#ifdef CERES_NO_CXSPARSE + if (options.sparse_linear_algebra_library_type == CX_SPARSE) { + if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY with CX_SPARSE because " + "CXSparse was not enabled when Ceres was built."; + return false; + } else if (options.linear_solver_type == SPARSE_SCHUR) { + *error = "Can't use SPARSE_SCHUR with CX_SPARSE because " + "CXSparse was not enabled when Ceres was built."; + return false; + } + } +#endif + +#ifndef CERES_USE_EIGEN_SPARSE + if (options.sparse_linear_algebra_library_type == EIGEN_SPARSE) { + if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY with EIGEN_SPARSE because " + "Eigen's sparse linear algebra was not enabled when Ceres was " + "built."; + return false; + } else if (options.linear_solver_type == SPARSE_SCHUR) { + *error = "Can't use SPARSE_SCHUR with EIGEN_SPARSE because " + "Eigen's sparse linear algebra was not enabled when Ceres was " + "built."; + return false; + } + } +#endif + + if (options.sparse_linear_algebra_library_type == NO_SPARSE) { + if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY as " + "sparse_linear_algebra_library_type is NO_SPARSE."; + return false; + } else if (options.linear_solver_type == SPARSE_SCHUR) { + *error = "Can't use SPARSE_SCHUR as " + "sparse_linear_algebra_library_type is NO_SPARSE."; + return false; + } + } + + if (options.trust_region_strategy_type == DOGLEG) { + if (options.linear_solver_type == ITERATIVE_SCHUR || + options.linear_solver_type == CGNR) { + *error = "DOGLEG only supports exact factorization based linear " + "solvers. If you want to use an iterative solver please " + "use LEVENBERG_MARQUARDT as the trust_region_strategy_type"; + return false; + } + } + + 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()) { + *error = "Solver::Options::trust_region_problem_dump_directory is empty."; + return false; + } + + if (options.dynamic_sparsity && + options.linear_solver_type != SPARSE_NORMAL_CHOLESKY) { + *error = "Dynamic sparsity is only supported with SPARSE_NORMAL_CHOLESKY."; + return false; + } + + return true; +} + +bool LineSearchOptionsAreValid(const Solver::Options& options, string* error) { + OPTION_GT(max_lbfgs_rank, 0); + OPTION_GT(min_line_search_step_size, 0.0); + OPTION_GT(max_line_search_step_contraction, 0.0); + OPTION_LT(max_line_search_step_contraction, 1.0); + OPTION_LT_OPTION(max_line_search_step_contraction, + min_line_search_step_contraction); + OPTION_LE(min_line_search_step_contraction, 1.0); + OPTION_GT(max_num_line_search_step_size_iterations, 0); + OPTION_GT(line_search_sufficient_function_decrease, 0.0); + OPTION_LT_OPTION(line_search_sufficient_function_decrease, + line_search_sufficient_curvature_decrease); + OPTION_LT(line_search_sufficient_curvature_decrease, 1.0); + OPTION_GT(max_line_search_step_expansion, 1.0); + + if ((options.line_search_direction_type == ceres::BFGS || + options.line_search_direction_type == ceres::LBFGS) && + options.line_search_type != ceres::WOLFE) { + *error = + string("Invalid configuration: Solver::Options::line_search_type = ") + + string(LineSearchTypeToString(options.line_search_type)) + + string(". When using (L)BFGS, " + "Solver::Options::line_search_type must be set to WOLFE."); + return false; + } + + // 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, + (options.line_search_interpolation_type == ceres::BISECTION && + (options.max_line_search_step_contraction > 0.5 || + options.min_line_search_step_contraction < 0.5))) + << "Line search interpolation type is BISECTION, but specified " + << "max_line_search_step_contraction: " + << options.max_line_search_step_contraction << ", and " + << "min_line_search_step_contraction: " + << options.min_line_search_step_contraction + << ", prevent bisection (0.5) scaling, continuing with solve regardless."; + + return true; +} + +#undef OPTION_OP +#undef OPTION_OP_OPTION +#undef OPTION_GT +#undef OPTION_GE +#undef OPTION_LE +#undef OPTION_LT +#undef OPTION_LE_OPTION +#undef OPTION_LT_OPTION + +void StringifyOrdering(const vector<int>& ordering, string* report) { + if (ordering.size() == 0) { + internal::StringAppendF(report, "AUTOMATIC"); + return; + } + + for (int i = 0; i < ordering.size() - 1; ++i) { + internal::StringAppendF(report, "%d, ", ordering[i]); + } + internal::StringAppendF(report, "%d", ordering.back()); +} + +void SummarizeGivenProgram(const internal::Program& program, + Solver::Summary* summary) { + summary->num_parameter_blocks = program.NumParameterBlocks(); + summary->num_parameters = program.NumParameters(); + summary->num_effective_parameters = program.NumEffectiveParameters(); + summary->num_residual_blocks = program.NumResidualBlocks(); + summary->num_residuals = program.NumResiduals(); +} + +void SummarizeReducedProgram(const internal::Program& program, + Solver::Summary* summary) { + summary->num_parameter_blocks_reduced = program.NumParameterBlocks(); + summary->num_parameters_reduced = program.NumParameters(); + summary->num_effective_parameters_reduced = program.NumEffectiveParameters(); + summary->num_residual_blocks_reduced = program.NumResidualBlocks(); + summary->num_residuals_reduced = program.NumResiduals(); +} + +void PreSolveSummarize(const Solver::Options& options, + const internal::ProblemImpl* problem, + Solver::Summary* summary) { + SummarizeGivenProgram(problem->program(), summary); + internal::OrderingToGroupSizes(options.linear_solver_ordering.get(), + &(summary->linear_solver_ordering_given)); + internal::OrderingToGroupSizes(options.inner_iteration_ordering.get(), + &(summary->inner_iteration_ordering_given)); + + summary->dense_linear_algebra_library_type = options.dense_linear_algebra_library_type; // NOLINT + summary->dogleg_type = options.dogleg_type; + summary->inner_iteration_time_in_seconds = 0.0; + summary->line_search_cost_evaluation_time_in_seconds = 0.0; + summary->line_search_gradient_evaluation_time_in_seconds = 0.0; + summary->line_search_polynomial_minimization_time_in_seconds = 0.0; + summary->line_search_total_time_in_seconds = 0.0; + summary->inner_iterations_given = options.use_inner_iterations; + summary->line_search_direction_type = options.line_search_direction_type; // NOLINT + summary->line_search_interpolation_type = options.line_search_interpolation_type; // NOLINT + summary->line_search_type = options.line_search_type; + summary->linear_solver_type_given = options.linear_solver_type; + summary->max_lbfgs_rank = options.max_lbfgs_rank; + summary->minimizer_type = options.minimizer_type; + summary->nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; // NOLINT + summary->num_linear_solver_threads_given = options.num_linear_solver_threads; // NOLINT + summary->num_threads_given = options.num_threads; + summary->preconditioner_type_given = options.preconditioner_type; + summary->sparse_linear_algebra_library_type = options.sparse_linear_algebra_library_type; // NOLINT + summary->trust_region_strategy_type = options.trust_region_strategy_type; // NOLINT + summary->visibility_clustering_type = options.visibility_clustering_type; // NOLINT +} + +void PostSolveSummarize(const internal::PreprocessedProblem& pp, + Solver::Summary* summary) { + internal::OrderingToGroupSizes(pp.options.linear_solver_ordering.get(), + &(summary->linear_solver_ordering_used)); + internal::OrderingToGroupSizes(pp.options.inner_iteration_ordering.get(), + &(summary->inner_iteration_ordering_used)); + + summary->inner_iterations_used = pp.inner_iteration_minimizer.get() != NULL; // NOLINT + summary->linear_solver_type_used = pp.options.linear_solver_type; + summary->num_linear_solver_threads_used = pp.options.num_linear_solver_threads; // NOLINT + summary->num_threads_used = pp.options.num_threads; + summary->preconditioner_type_used = pp.options.preconditioner_type; // NOLINT + + internal::SetSummaryFinalCost(summary); + + if (pp.reduced_program.get() != NULL) { + SummarizeReducedProgram(*pp.reduced_program, summary); + } + + // It is possible that no evaluator was created. This would be the + // case if the preprocessor failed, or if the reduced problem did + // not contain any parameter blocks. Thus, only extract the + // evaluator statistics if one exists. + if (pp.evaluator.get() != NULL) { + const map<string, double>& evaluator_time_statistics = + pp.evaluator->TimeStatistics(); + summary->residual_evaluation_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0); + summary->jacobian_evaluation_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0); + } + + // Again, like the evaluator, there may or may not be a linear + // solver from which we can extract run time statistics. In + // particular the line search solver does not use a linear solver. + if (pp.linear_solver.get() != NULL) { + const map<string, double>& linear_solver_time_statistics = + pp.linear_solver->TimeStatistics(); + summary->linear_solver_time_in_seconds = + FindWithDefault(linear_solver_time_statistics, + "LinearSolver::Solve", + 0.0); + } +} + +void Minimize(internal::PreprocessedProblem* pp, + Solver::Summary* summary) { + using internal::Program; + using internal::scoped_ptr; + using internal::Minimizer; + + Program* program = pp->reduced_program.get(); + if (pp->reduced_program->NumParameterBlocks() == 0) { + summary->message = "Function tolerance reached. " + "No non-constant parameter blocks found."; + summary->termination_type = CONVERGENCE; + VLOG_IF(1, pp->options.logging_type != SILENT) << summary->message; + summary->initial_cost = summary->fixed_cost; + summary->final_cost = summary->fixed_cost; + return; + } + + scoped_ptr<Minimizer> minimizer( + Minimizer::Create(pp->options.minimizer_type)); + minimizer->Minimize(pp->minimizer_options, + pp->reduced_parameters.data(), + summary); + + if (summary->IsSolutionUsable()) { + program->StateVectorToParameterBlocks(pp->reduced_parameters.data()); + program->CopyParameterBlockStateToUserState(); + } +} + +} // namespace + +bool Solver::Options::IsValid(string* error) const { + if (!CommonOptionsAreValid(*this, error)) { + return false; + } + + if (minimizer_type == TRUST_REGION && + !TrustRegionOptionsAreValid(*this, error)) { + return false; + } + + // We do not know if the problem is bounds constrained or not, if it + // is then the trust region solver will also use the line search + // solver to do a projection onto the box constraints, so make sure + // that the line search options are checked independent of what + // minimizer algorithm is being used. + return LineSearchOptionsAreValid(*this, error); +} + +Solver::~Solver() {} + +void Solver::Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary) { + using internal::PreprocessedProblem; + using internal::Preprocessor; + using internal::ProblemImpl; + using internal::Program; + using internal::scoped_ptr; + using internal::WallTimeInSeconds; + + CHECK_NOTNULL(problem); + CHECK_NOTNULL(summary); + + double start_time = WallTimeInSeconds(); + *summary = Summary(); + if (!options.IsValid(&summary->message)) { + LOG(ERROR) << "Terminating: " << summary->message; + return; + } + + ProblemImpl* problem_impl = problem->problem_impl_.get(); + Program* program = problem_impl->mutable_program(); + PreSolveSummarize(options, problem_impl, summary); + + // Make sure that all the parameter blocks states are set to the + // values provided by the user. + program->SetParameterBlockStatePtrsToUserStatePtrs(); + + scoped_ptr<internal::ProblemImpl> gradient_checking_problem; + if (options.check_gradients) { + gradient_checking_problem.reset( + CreateGradientCheckingProblemImpl( + problem_impl, + options.numeric_derivative_relative_step_size, + options.gradient_check_relative_precision)); + problem_impl = gradient_checking_problem.get(); + program = problem_impl->mutable_program(); + } + + scoped_ptr<Preprocessor> preprocessor( + Preprocessor::Create(options.minimizer_type)); + PreprocessedProblem pp; + const bool status = preprocessor->Preprocess(options, problem_impl, &pp); + summary->fixed_cost = pp.fixed_cost; + summary->preprocessor_time_in_seconds = WallTimeInSeconds() - start_time; + + if (status) { + const double minimizer_start_time = WallTimeInSeconds(); + Minimize(&pp, summary); + summary->minimizer_time_in_seconds = + WallTimeInSeconds() - minimizer_start_time; + } else { + summary->message = pp.error; + } + + const double postprocessor_start_time = WallTimeInSeconds(); + problem_impl = problem->problem_impl_.get(); + program = problem_impl->mutable_program(); + // On exit, ensure that the parameter blocks again point at the user + // provided values and the parameter blocks are numbered according + // to their position in the original user provided program. + program->SetParameterBlockStatePtrsToUserStatePtrs(); + program->SetParameterOffsetsAndIndex(); + PostSolveSummarize(pp, summary); + summary->postprocessor_time_in_seconds = + WallTimeInSeconds() - postprocessor_start_time; + + summary->total_time_in_seconds = WallTimeInSeconds() - start_time; +} + +void Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary) { + Solver solver; + solver.Solve(options, problem, summary); +} + +Solver::Summary::Summary() + // Invalid values for most fields, to ensure that we are not + // accidentally reporting default values. + : minimizer_type(TRUST_REGION), + termination_type(FAILURE), + message("ceres::Solve was not called."), + initial_cost(-1.0), + final_cost(-1.0), + 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), + total_time_in_seconds(-1.0), + 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), + line_search_cost_evaluation_time_in_seconds(-1.0), + line_search_gradient_evaluation_time_in_seconds(-1.0), + line_search_polynomial_minimization_time_in_seconds(-1.0), + line_search_total_time_in_seconds(-1.0), + num_parameter_blocks(-1), + num_parameters(-1), + num_effective_parameters(-1), + num_residual_blocks(-1), + num_residuals(-1), + num_parameter_blocks_reduced(-1), + num_parameters_reduced(-1), + num_effective_parameters_reduced(-1), + num_residual_blocks_reduced(-1), + num_residuals_reduced(-1), + is_constrained(false), + num_threads_given(-1), + num_threads_used(-1), + num_linear_solver_threads_given(-1), + num_linear_solver_threads_used(-1), + linear_solver_type_given(SPARSE_NORMAL_CHOLESKY), + linear_solver_type_used(SPARSE_NORMAL_CHOLESKY), + inner_iterations_given(false), + inner_iterations_used(false), + preconditioner_type_given(IDENTITY), + preconditioner_type_used(IDENTITY), + visibility_clustering_type(CANONICAL_VIEWS), + trust_region_strategy_type(LEVENBERG_MARQUARDT), + dense_linear_algebra_library_type(EIGEN), + sparse_linear_algebra_library_type(SUITE_SPARSE), + line_search_direction_type(LBFGS), + line_search_type(ARMIJO), + line_search_interpolation_type(BISECTION), + nonlinear_conjugate_gradient_type(FLETCHER_REEVES), + max_lbfgs_rank(-1) { +} + +using internal::StringAppendF; +using internal::StringPrintf; + +string Solver::Summary::BriefReport() const { + return StringPrintf("Ceres Solver Report: " + "Iterations: %d, " + "Initial cost: %e, " + "Final cost: %e, " + "Termination: %s", + num_successful_steps + num_unsuccessful_steps, + initial_cost, + final_cost, + TerminationTypeToString(termination_type)); +} + +string Solver::Summary::FullReport() const { + using internal::VersionString; + + string report = string("\nSolver Summary (v " + VersionString() + ")\n\n"); + + StringAppendF(&report, "%45s %21s\n", "Original", "Reduced"); + StringAppendF(&report, "Parameter blocks % 25d% 25d\n", + num_parameter_blocks, num_parameter_blocks_reduced); + StringAppendF(&report, "Parameters % 25d% 25d\n", + num_parameters, num_parameters_reduced); + if (num_effective_parameters_reduced != num_parameters_reduced) { + StringAppendF(&report, "Effective parameters% 25d% 25d\n", + num_effective_parameters, num_effective_parameters_reduced); + } + StringAppendF(&report, "Residual blocks % 25d% 25d\n", + num_residual_blocks, num_residual_blocks_reduced); + StringAppendF(&report, "Residual % 25d% 25d\n", + num_residuals, num_residuals_reduced); + + 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_used == CLUSTER_JACOBI || + preconditioner_type_used == CLUSTER_TRIDIAGONAL))) { + StringAppendF(&report, "\nSparse linear algebra library %15s\n", + SparseLinearAlgebraLibraryTypeToString( + sparse_linear_algebra_library_type)); + } + + StringAppendF(&report, "Trust region strategy %19s", + TrustRegionStrategyTypeToString( + trust_region_strategy_type)); + if (trust_region_strategy_type == DOGLEG) { + if (dogleg_type == TRADITIONAL_DOGLEG) { + StringAppendF(&report, " (TRADITIONAL)"); + } else { + StringAppendF(&report, " (SUBSPACE)"); + } + } + StringAppendF(&report, "\n"); + StringAppendF(&report, "\n"); + + StringAppendF(&report, "%45s %21s\n", "Given", "Used"); + StringAppendF(&report, "Linear solver %25s%25s\n", + LinearSolverTypeToString(linear_solver_type_given), + LinearSolverTypeToString(linear_solver_type_used)); + + if (linear_solver_type_given == CGNR || + linear_solver_type_given == ITERATIVE_SCHUR) { + StringAppendF(&report, "Preconditioner %25s%25s\n", + PreconditionerTypeToString(preconditioner_type_given), + PreconditionerTypeToString(preconditioner_type_used)); + } + + if (preconditioner_type_used == CLUSTER_JACOBI || + preconditioner_type_used == CLUSTER_TRIDIAGONAL) { + StringAppendF(&report, "Visibility clustering%24s%25s\n", + VisibilityClusteringTypeToString( + visibility_clustering_type), + VisibilityClusteringTypeToString( + visibility_clustering_type)); + } + 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, + num_linear_solver_threads_used); + + if (IsSchurType(linear_solver_type_used)) { + string given; + StringifyOrdering(linear_solver_ordering_given, &given); + string used; + StringifyOrdering(linear_solver_ordering_used, &used); + StringAppendF(&report, + "Linear solver ordering %22s %24s\n", + given.c_str(), + used.c_str()); + } + + 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; + StringifyOrdering(inner_iteration_ordering_used, &used); + StringAppendF(&report, + "Inner iteration ordering %20s %24s\n", + given.c_str(), + used.c_str()); + } + } else { + // LINE_SEARCH HEADER + StringAppendF(&report, "\nMinimizer %19s\n", "LINE_SEARCH"); + + + string line_search_direction_string; + if (line_search_direction_type == LBFGS) { + 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 { + line_search_direction_string = + LineSearchDirectionTypeToString(line_search_direction_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", + num_threads_given, num_threads_used); + } + + StringAppendF(&report, "\nCost:\n"); + StringAppendF(&report, "Initial % 30e\n", initial_cost); + if (termination_type != FAILURE && + termination_type != USER_FAILURE) { + StringAppendF(&report, "Final % 30e\n", final_cost); + StringAppendF(&report, "Change % 30e\n", + initial_cost - final_cost); + } + + StringAppendF(&report, "\nMinimizer iterations % 16d\n", + num_successful_steps + num_unsuccessful_steps); + + // 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); + } + + const bool print_line_search_timing_information = + minimizer_type == LINE_SEARCH || + (minimizer_type == TRUST_REGION && is_constrained); + + StringAppendF(&report, "\nTime (in seconds):\n"); + StringAppendF(&report, "Preprocessor %25.4f\n", + preprocessor_time_in_seconds); + + StringAppendF(&report, "\n Residual evaluation %23.4f\n", + residual_evaluation_time_in_seconds); + if (print_line_search_timing_information) { + StringAppendF(&report, " Line search cost evaluation %10.4f\n", + line_search_cost_evaluation_time_in_seconds); + } + StringAppendF(&report, " Jacobian evaluation %23.4f\n", + jacobian_evaluation_time_in_seconds); + if (print_line_search_timing_information) { + StringAppendF(&report, " Line search gradient evaluation %6.4f\n", + line_search_gradient_evaluation_time_in_seconds); + } + + if (minimizer_type == TRUST_REGION) { + StringAppendF(&report, " Linear solver %23.4f\n", + linear_solver_time_in_seconds); + } + + if (inner_iterations_used) { + StringAppendF(&report, " Inner iterations %23.4f\n", + inner_iteration_time_in_seconds); + } + + if (print_line_search_timing_information) { + StringAppendF(&report, " Line search polynomial minimization %.4f\n", + line_search_polynomial_minimization_time_in_seconds); + } + + StringAppendF(&report, "Minimizer %25.4f\n\n", + minimizer_time_in_seconds); + + StringAppendF(&report, "Postprocessor %24.4f\n", + postprocessor_time_in_seconds); + + StringAppendF(&report, "Total %25.4f\n\n", + total_time_in_seconds); + + StringAppendF(&report, "Termination: %25s (%s)\n", + TerminationTypeToString(termination_type), message.c_str()); + return report; +} + +bool Solver::Summary::IsSolutionUsable() const { + return internal::IsSolutionUsable(*this); +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/solver_utils.cc b/extern/ceres/internal/ceres/solver_utils.cc new file mode 100644 index 00000000000..7f4ff7eb940 --- /dev/null +++ b/extern/ceres/internal/ceres/solver_utils.cc @@ -0,0 +1,86 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <string> + +#include "Eigen/Core" +#include "ceres/internal/port.h" +#include "ceres/solver_utils.h" +#include "ceres/version.h" + +namespace ceres { +namespace internal { + +#define CERES_EIGEN_VERSION \ + CERES_TO_STRING(EIGEN_WORLD_VERSION) "." \ + CERES_TO_STRING(EIGEN_MAJOR_VERSION) "." \ + CERES_TO_STRING(EIGEN_MINOR_VERSION) + +std::string VersionString() { + std::string value = std::string(CERES_VERSION_STRING); + value += "-eigen-(" + std::string(CERES_EIGEN_VERSION) + ")"; + +#ifdef CERES_NO_LAPACK + value += "-no_lapack"; +#else + value += "-lapack"; +#endif + +#ifndef CERES_NO_SUITESPARSE + value += "-suitesparse-(" + std::string(CERES_SUITESPARSE_VERSION) + ")"; +#endif + +#ifndef CERES_NO_CXSPARSE + value += "-cxsparse-(" + std::string(CERES_CXSPARSE_VERSION) + ")"; +#endif + +#ifdef CERES_USE_EIGEN_SPARSE + value += "-eigensparse"; +#endif + +#ifdef CERES_RESTRUCT_SCHUR_SPECIALIZATIONS + value += "-no_schur_specializations"; +#endif + +#ifdef CERES_USE_OPENMP + value += "-openmp"; +#else + value += "-no_openmp"; +#endif + +#ifdef CERES_NO_CUSTOM_BLAS + value += "-no_custom_blas"; +#endif + + return value; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/solver_utils.h b/extern/ceres/internal/ceres/solver_utils.h new file mode 100644 index 00000000000..85fbf3776ab --- /dev/null +++ b/extern/ceres/internal/ceres/solver_utils.h @@ -0,0 +1,61 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <algorithm> +#include <string> + +#include "ceres/iteration_callback.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +template <typename SummaryType> +bool IsSolutionUsable(const SummaryType& summary) { + return (summary.termination_type == CONVERGENCE || + summary.termination_type == NO_CONVERGENCE || + summary.termination_type == USER_SUCCESS); +} + +template <typename SummaryType> +void SetSummaryFinalCost(SummaryType* summary) { + summary->final_cost = summary->initial_cost; + // We need the loop here, instead of just looking at the last + // iteration because the minimizer maybe making non-monotonic steps. + for (int i = 0; i < summary->iterations.size(); ++i) { + const IterationSummary& iteration_summary = summary->iterations[i]; + summary->final_cost = std::min(iteration_summary.cost, summary->final_cost); + } +} + +std::string VersionString(); + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/sparse_matrix.cc b/extern/ceres/internal/ceres/sparse_matrix.cc new file mode 100644 index 00000000000..f95ff3220bd --- /dev/null +++ b/extern/ceres/internal/ceres/sparse_matrix.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/sparse_matrix.h" + +namespace ceres { +namespace internal { + +SparseMatrix::~SparseMatrix() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/sparse_matrix.h b/extern/ceres/internal/ceres/sparse_matrix.h new file mode 100644 index 00000000000..b3af1d06440 --- /dev/null +++ b/extern/ceres/internal/ceres/sparse_matrix.h @@ -0,0 +1,107 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Interface definition for sparse matrices. + +#ifndef CERES_INTERNAL_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_SPARSE_MATRIX_H_ + +#include <cstdio> +#include "ceres/linear_operator.h" +#include "ceres/internal/eigen.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// This class defines the interface for storing and manipulating +// sparse matrices. The key property that differentiates different +// sparse matrices is how they are organized in memory and how the +// information about the sparsity structure of the matrix is +// stored. This has significant implications for linear solvers +// operating on these matrices. +// +// To deal with the different kinds of layouts, we will assume that a +// sparse matrix will have a two part representation. A values array +// that will be used to store the entries of the sparse matrix and +// some sort of a layout object that tells the user the sparsity +// structure and layout of the values array. For example in case of +// the TripletSparseMatrix, this information is carried in the rows +// and cols arrays and for the BlockSparseMatrix, this information is +// carried in the CompressedRowBlockStructure object. +// +// This interface deliberately does not contain any information about +// the structure of the sparse matrix as that seems to be highly +// matrix type dependent and we are at this stage unable to come up +// with an efficient high level interface that spans multiple sparse +// matrix types. +class SparseMatrix : public LinearOperator { + public: + virtual ~SparseMatrix(); + + // y += Ax; + virtual void RightMultiply(const double* x, double* y) const = 0; + // y += A'x; + virtual void LeftMultiply(const double* x, double* y) const = 0; + + // In MATLAB notation sum(A.*A, 1) + virtual void SquaredColumnNorm(double* x) const = 0; + // A = A * diag(scale) + virtual void ScaleColumns(const double* scale) = 0; + + // A = 0. A->num_nonzeros() == 0 is true after this call. The + // sparsity pattern is preserved. + virtual void SetZero() = 0; + + // Resize and populate dense_matrix with a dense version of the + // sparse matrix. + virtual void ToDenseMatrix(Matrix* dense_matrix) const = 0; + + // Write out the matrix as a sequence of (i,j,s) triplets. This + // format is useful for loading the matrix into MATLAB/octave as a + // sparse matrix. + virtual void ToTextFile(FILE* file) const = 0; + + // Accessors for the values array that stores the entries of the + // sparse matrix. The exact interpreptation of the values of this + // array depends on the particular kind of SparseMatrix being + // accessed. + virtual double* mutable_values() = 0; + virtual const double* values() const = 0; + + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; + virtual int num_nonzeros() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SPARSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc new file mode 100644 index 00000000000..ed00879b47a --- /dev/null +++ b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc @@ -0,0 +1,486 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/sparse_normal_cholesky_solver.h" + +#include <algorithm> +#include <cstring> +#include <ctime> + +#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" +#include "ceres/suitesparse.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "ceres/wall_time.h" +#include "Eigen/SparseCore" + +#ifdef CERES_USE_EIGEN_SPARSE +#include "Eigen/SparseCholesky" +#endif + +namespace ceres { +namespace internal { +namespace { + +#ifdef CERES_USE_EIGEN_SPARSE +// A templated factorized and solve function, which allows us to use +// the same code independent of whether a AMD or a Natural ordering is +// used. +template <typename SimplicialCholeskySolver, typename SparseMatrixType> +LinearSolver::Summary SimplicialLDLTSolve( + const SparseMatrixType& lhs, + const bool do_symbolic_analysis, + SimplicialCholeskySolver* solver, + double* rhs_and_solution, + EventLogger* event_logger) { + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + if (do_symbolic_analysis) { + solver->analyzePattern(lhs); + event_logger->AddEvent("Analyze"); + if (solver->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "Eigen failure. Unable to find symbolic factorization."; + return summary; + } + } + + solver->factorize(lhs); + event_logger->AddEvent("Factorize"); + if (solver->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Eigen failure. Unable to find numeric factorization."; + return summary; + } + + const Vector rhs = VectorRef(rhs_and_solution, lhs.cols()); + + VectorRef(rhs_and_solution, lhs.cols()) = solver->solve(rhs); + event_logger->AddEvent("Solve"); + if (solver->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Eigen failure. Unable to do triangular solve."; + return summary; + } + + return summary; +} + +#endif // CERES_USE_EIGEN_SPARSE + +#ifndef CERES_NO_CXSPARSE +LinearSolver::Summary ComputeNormalEquationsAndSolveUsingCXSparse( + CompressedRowSparseMatrix* A, + double * rhs_and_solution, + EventLogger* event_logger) { + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + CXSparse cxsparse; + + // Wrap the augmented Jacobian in a compressed sparse column matrix. + cs_di a_transpose = cxsparse.CreateSparseMatrixTransposeView(A); + + // Compute the normal equations. J'J delta = J'f and solve them + // using a sparse Cholesky factorization. Notice that when compared + // to SuiteSparse we have to explicitly compute the transpose of Jt, + // and then the normal equations before they can be + // factorized. CHOLMOD/SuiteSparse on the other hand can just work + // off of Jt to compute the Cholesky factorization of the normal + // equations. + cs_di* a = cxsparse.TransposeMatrix(&a_transpose); + cs_di* lhs = cxsparse.MatrixMatrixMultiply(&a_transpose, a); + cxsparse.Free(a); + event_logger->AddEvent("NormalEquations"); + + cs_dis* factor = cxsparse.AnalyzeCholesky(lhs); + event_logger->AddEvent("Analysis"); + + if (factor == NULL) { + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = "CXSparse::AnalyzeCholesky failed."; + } else if (!cxsparse.SolveCholesky(lhs, factor, rhs_and_solution)) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "CXSparse::SolveCholesky failed."; + } + event_logger->AddEvent("Solve"); + + cxsparse.Free(lhs); + cxsparse.Free(factor); + event_logger->AddEvent("TearDown"); + return summary; +} + +#endif // CERES_NO_CXSPARSE + +} // namespace + +SparseNormalCholeskySolver::SparseNormalCholeskySolver( + const LinearSolver::Options& options) + : factor_(NULL), + cxsparse_factor_(NULL), + options_(options) { +} + +void SparseNormalCholeskySolver::FreeFactorization() { + if (factor_ != NULL) { + ss_.Free(factor_); + factor_ = NULL; + } + + if (cxsparse_factor_ != NULL) { + cxsparse_.Free(cxsparse_factor_); + cxsparse_factor_ = NULL; + } +} + +SparseNormalCholeskySolver::~SparseNormalCholeskySolver() { + FreeFactorization(); +} + +LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double * x) { + + const int num_cols = A->num_cols(); + VectorRef(x, num_cols).setZero(); + A->LeftMultiply(b, x); + + 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. + scoped_ptr<CompressedRowSparseMatrix> regularizer; + if (A->col_blocks().size() > 0) { + regularizer.reset(CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( + per_solve_options.D, A->col_blocks())); + } else { + regularizer.reset(new CompressedRowSparseMatrix( + per_solve_options.D, num_cols)); + } + A->AppendRows(*regularizer); + } + + LinearSolver::Summary summary; + switch (options_.sparse_linear_algebra_library_type) { + case SUITE_SPARSE: + summary = SolveImplUsingSuiteSparse(A, x); + break; + case CX_SPARSE: + summary = SolveImplUsingCXSparse(A, x); + break; + case EIGEN_SPARSE: + summary = SolveImplUsingEigen(A, x); + break; + default: + LOG(FATAL) << "Unknown sparse linear algebra library : " + << options_.sparse_linear_algebra_library_type; + } + + if (per_solve_options.D != NULL) { + A->DeleteRows(num_cols); + } + + return summary; +} + +LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen( + CompressedRowSparseMatrix* A, + double * rhs_and_solution) { +#ifndef CERES_USE_EIGEN_SPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE " + "because Ceres was not built with support for " + "Eigen's SimplicialLDLT decomposition. " + "This requires enabling building with -DEIGENSPARSE=ON."; + return summary; + +#else + + EventLogger event_logger("SparseNormalCholeskySolver::Eigen::Solve"); + // Compute the normal equations. J'J delta = J'f and solve them + // using a sparse Cholesky factorization. Notice that when compared + // to SuiteSparse we have to explicitly compute the normal equations + // before they can be factorized. CHOLMOD/SuiteSparse on the other + // hand can just work off of Jt to compute the Cholesky + // factorization of the normal equations. + + if (options_.dynamic_sparsity) { + // In the case where the problem has dynamic sparsity, it is not + // worth using the ComputeOuterProduct routine, as the setup cost + // is not amortized over multiple calls to Solve. + Eigen::MappedSparseMatrix<double, Eigen::RowMajor> a( + A->num_rows(), + A->num_cols(), + A->num_nonzeros(), + A->mutable_rows(), + A->mutable_cols(), + A->mutable_values()); + + Eigen::SparseMatrix<double> lhs = a.transpose() * a; + Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver; + return SimplicialLDLTSolve(lhs, + true, + &solver, + rhs_and_solution, + &event_logger); + } + + if (outer_product_.get() == NULL) { + outer_product_.reset( + CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( + *A, &pattern_)); + } + + CompressedRowSparseMatrix::ComputeOuterProduct( + *A, pattern_, outer_product_.get()); + + // Map to an upper triangular column major matrix. + // + // outer_product_ is a compressed row sparse matrix and in lower + // triangular form, when mapped to a compressed column sparse + // matrix, it becomes an upper triangular matrix. + Eigen::MappedSparseMatrix<double, Eigen::ColMajor> lhs( + outer_product_->num_rows(), + outer_product_->num_rows(), + outer_product_->num_nonzeros(), + outer_product_->mutable_rows(), + outer_product_->mutable_cols(), + outer_product_->mutable_values()); + + bool do_symbolic_analysis = false; + + // If using post ordering or an old version of Eigen, we cannot + // depend on a preordered jacobian, so we work with a SimplicialLDLT + // decomposition with AMD ordering. + if (options_.use_postordering || + !EIGEN_VERSION_AT_LEAST(3, 2, 2)) { + if (amd_ldlt_.get() == NULL) { + amd_ldlt_.reset(new SimplicialLDLTWithAMDOrdering); + do_symbolic_analysis = true; + } + + return SimplicialLDLTSolve(lhs, + do_symbolic_analysis, + amd_ldlt_.get(), + rhs_and_solution, + &event_logger); + } + +#if EIGEN_VERSION_AT_LEAST(3,2,2) + // The common case + if (natural_ldlt_.get() == NULL) { + natural_ldlt_.reset(new SimplicialLDLTWithNaturalOrdering); + do_symbolic_analysis = true; + } + + return SimplicialLDLTSolve(lhs, + do_symbolic_analysis, + natural_ldlt_.get(), + rhs_and_solution, + &event_logger); +#endif + +#endif // EIGEN_USE_EIGEN_SPARSE +} + +LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( + CompressedRowSparseMatrix* A, + double * rhs_and_solution) { +#ifdef CERES_NO_CXSPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "SPARSE_NORMAL_CHOLESKY cannot be used with CX_SPARSE " + "because Ceres was not built with support for CXSparse. " + "This requires enabling building with -DCXSPARSE=ON."; + + return summary; + +#else + + EventLogger event_logger("SparseNormalCholeskySolver::CXSparse::Solve"); + if (options_.dynamic_sparsity) { + return ComputeNormalEquationsAndSolveUsingCXSparse(A, + rhs_and_solution, + &event_logger); + } + + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + // Compute the normal equations. J'J delta = J'f and solve them + // using a sparse Cholesky factorization. Notice that when compared + // to SuiteSparse we have to explicitly compute the normal equations + // before they can be factorized. CHOLMOD/SuiteSparse on the other + // hand can just work off of Jt to compute the Cholesky + // factorization of the normal equations. + if (outer_product_.get() == NULL) { + outer_product_.reset( + CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( + *A, &pattern_)); + } + + CompressedRowSparseMatrix::ComputeOuterProduct( + *A, pattern_, outer_product_.get()); + cs_di lhs = + cxsparse_.CreateSparseMatrixTransposeView(outer_product_.get()); + + event_logger.AddEvent("Setup"); + + // Compute symbolic factorization if not available. + if (cxsparse_factor_ == NULL) { + if (options_.use_postordering) { + cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(&lhs, + A->col_blocks(), + A->col_blocks()); + } else { + cxsparse_factor_ = cxsparse_.AnalyzeCholeskyWithNaturalOrdering(&lhs); + } + } + event_logger.AddEvent("Analysis"); + + if (cxsparse_factor_ == NULL) { + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "CXSparse failure. Unable to find symbolic factorization."; + } else if (!cxsparse_.SolveCholesky(&lhs, + cxsparse_factor_, + rhs_and_solution)) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "CXSparse::SolveCholesky failed."; + } + event_logger.AddEvent("Solve"); + + return summary; +#endif +} + +LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( + CompressedRowSparseMatrix* A, + double * rhs_and_solution) { +#ifdef CERES_NO_SUITESPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "SPARSE_NORMAL_CHOLESKY cannot be used with SUITE_SPARSE " + "because Ceres was not built with support for SuiteSparse. " + "This requires enabling building with -DSUITESPARSE=ON."; + return summary; + +#else + + EventLogger event_logger("SparseNormalCholeskySolver::SuiteSparse::Solve"); + LinearSolver::Summary summary; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.num_iterations = 1; + summary.message = "Success."; + + const int num_cols = A->num_cols(); + cholmod_sparse lhs = ss_.CreateSparseMatrixTransposeView(A); + event_logger.AddEvent("Setup"); + + if (options_.dynamic_sparsity) { + FreeFactorization(); + } + + if (factor_ == NULL) { + if (options_.use_postordering) { + factor_ = ss_.BlockAnalyzeCholesky(&lhs, + A->col_blocks(), + A->row_blocks(), + &summary.message); + } else { + if (options_.dynamic_sparsity) { + factor_ = ss_.AnalyzeCholesky(&lhs, &summary.message); + } else { + factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs, + &summary.message); + } + } + } + event_logger.AddEvent("Analysis"); + + if (factor_ == NULL) { + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + // No need to set message as it has already been set by the + // symbolic analysis routines above. + return summary; + } + + summary.termination_type = ss_.Cholesky(&lhs, factor_, &summary.message); + if (summary.termination_type != LINEAR_SOLVER_SUCCESS) { + return summary; + } + + cholmod_dense* rhs = ss_.CreateDenseVector(rhs_and_solution, + num_cols, + num_cols); + cholmod_dense* solution = ss_.Solve(factor_, rhs, &summary.message); + event_logger.AddEvent("Solve"); + + ss_.Free(rhs); + if (solution != NULL) { + memcpy(rhs_and_solution, solution->x, num_cols * sizeof(*rhs_and_solution)); + ss_.Free(solution); + } else { + // No need to set message as it has already been set by the + // numeric factorization routine above. + summary.termination_type = LINEAR_SOLVER_FAILURE; + } + + event_logger.AddEvent("Teardown"); + return summary; +#endif +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.h b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.h new file mode 100644 index 00000000000..2a93bc56d29 --- /dev/null +++ b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.h @@ -0,0 +1,127 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A solver for sparse linear least squares problem based on solving +// the normal equations via a sparse cholesky factorization. + +#ifndef CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ +#define CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ + +#include <vector> + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#include "ceres/internal/macros.h" +#include "ceres/linear_solver.h" +#include "ceres/suitesparse.h" +#include "ceres/cxsparse.h" + +#ifdef CERES_USE_EIGEN_SPARSE +#include "Eigen/SparseCholesky" +#endif + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; + +// Solves the normal equations (A'A + D'D) x = A'b, using the CHOLMOD sparse +// cholesky solver. +class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { + public: + explicit SparseNormalCholeskySolver(const LinearSolver::Options& options); + virtual ~SparseNormalCholeskySolver(); + + private: + virtual LinearSolver::Summary SolveImpl( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& options, + double* x); + + LinearSolver::Summary SolveImplUsingSuiteSparse( + CompressedRowSparseMatrix* A, + double* rhs_and_solution); + + + LinearSolver::Summary SolveImplUsingCXSparse( + CompressedRowSparseMatrix* A, + double* rhs_and_solution); + + LinearSolver::Summary SolveImplUsingEigen( + CompressedRowSparseMatrix* A, + double* rhs_and_solution); + + void FreeFactorization(); + + SuiteSparse ss_; + // Cached factorization + cholmod_factor* factor_; + + CXSparse cxsparse_; + // Cached factorization + cs_dis* cxsparse_factor_; + +#ifdef CERES_USE_EIGEN_SPARSE + + // The preprocessor gymnastics here are dealing with the fact that + // before version 3.2.2, Eigen did not support a third template + // parameter to specify the ordering. +#if EIGEN_VERSION_AT_LEAST(3,2,2) + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper, + Eigen::NaturalOrdering<int> > + SimplicialLDLTWithNaturalOrdering; + scoped_ptr<SimplicialLDLTWithNaturalOrdering> natural_ldlt_; + + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper, + Eigen::AMDOrdering<int> > + SimplicialLDLTWithAMDOrdering; + scoped_ptr<SimplicialLDLTWithAMDOrdering> amd_ldlt_; + +#else + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper> + SimplicialLDLTWithAMDOrdering; + + scoped_ptr<SimplicialLDLTWithAMDOrdering> amd_ldlt_; +#endif + +#endif + + scoped_ptr<CompressedRowSparseMatrix> outer_product_; + std::vector<int> pattern_; + const LinearSolver::Options options_; + CERES_DISALLOW_COPY_AND_ASSIGN(SparseNormalCholeskySolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/split.cc b/extern/ceres/internal/ceres/split.cc new file mode 100644 index 00000000000..296c09a6440 --- /dev/null +++ b/extern/ceres/internal/ceres/split.cc @@ -0,0 +1,123 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/split.h" + +#include <iterator> +#include <string> +#include <vector> + +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +using std::string; +using std::vector; + +// If we know how much to allocate for a vector of strings, we can allocate the +// vector<string> only once and directly to the right size. This saves in +// between 33-66 % of memory space needed for the result, and runs faster in the +// microbenchmarks. +// +// The reserve is only implemented for the single character delim. +// +// The implementation for counting is cut-and-pasted from +// SplitStringToIteratorUsing. I could have written my own counting iterator, +// and use the existing template function, but probably this is more clear and +// more sure to get optimized to reasonable code. +static int CalculateReserveForVector(const string& full, const char* delim) { + int count = 0; + if (delim[0] != '\0' && delim[1] == '\0') { + // Optimize the common case where delim is a single character. + char c = delim[0]; + const char* p = full.data(); + const char* end = p + full.size(); + while (p != end) { + if (*p == c) { // This could be optimized with hasless(v,1) trick. + ++p; + } else { + while (++p != end && *p != c) { + // Skip to the next occurence of the delimiter. + } + ++count; + } + } + } + return count; +} + +template <typename StringType, typename ITR> +static inline +void SplitStringToIteratorUsing(const StringType& full, + const char* delim, + ITR& result) { + // Optimize the common case where delim is a single character. + if (delim[0] != '\0' && delim[1] == '\0') { + char c = delim[0]; + const char* p = full.data(); + const char* end = p + full.size(); + while (p != end) { + if (*p == c) { + ++p; + } else { + const char* start = p; + while (++p != end && *p != c) { + // Skip to the next occurence of the delimiter. + } + *result++ = StringType(start, p - start); + } + } + return; + } + + string::size_type begin_index, end_index; + begin_index = full.find_first_not_of(delim); + while (begin_index != string::npos) { + end_index = full.find_first_of(delim, begin_index); + if (end_index == string::npos) { + *result++ = full.substr(begin_index); + return; + } + *result++ = full.substr(begin_index, (end_index - begin_index)); + begin_index = full.find_first_not_of(delim, end_index); + } +} + +void SplitStringUsing(const string& full, + const char* delim, + vector<string>* result) { + result->reserve(result->size() + CalculateReserveForVector(full, delim)); + std::back_insert_iterator<vector<string> > it(*result); + SplitStringToIteratorUsing(full, delim, it); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/split.h b/extern/ceres/internal/ceres/split.h new file mode 100644 index 00000000000..94b773dee4d --- /dev/null +++ b/extern/ceres/internal/ceres/split.h @@ -0,0 +1,50 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_ +#define CERES_INTERNAL_SPLIT_H_ + +#include <string> +#include <vector> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +// Split a string using one or more character delimiters, presented as a +// nul-terminated c string. Append the components to 'result'. If there are +// consecutive delimiters, this function skips over all of them. +void SplitStringUsing(const std::string& full, const char* delim, + std::vector<std::string>* res); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SPLIT_H_ diff --git a/extern/ceres/internal/ceres/stl_util.h b/extern/ceres/internal/ceres/stl_util.h new file mode 100644 index 00000000000..0595a4cf2e9 --- /dev/null +++ b/extern/ceres/internal/ceres/stl_util.h @@ -0,0 +1,91 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_STL_UTIL_H_ +#define CERES_INTERNAL_STL_UTIL_H_ + +#include <algorithm> + +namespace ceres { + +// STLDeleteContainerPointers() +// For a range within a container of pointers, calls delete +// (non-array version) on these pointers. +// NOTE: for these three functions, we could just implement a DeleteObject +// functor and then call for_each() on the range and functor, but this +// requires us to pull in all of algorithm.h, which seems expensive. +// For hash_[multi]set, it is important that this deletes behind the iterator +// because the hash_set may call the hash function on the iterator when it is +// advanced, which could result in the hash function trying to deference a +// stale pointer. +template <class ForwardIterator> +void STLDeleteContainerPointers(ForwardIterator begin, + ForwardIterator end) { + while (begin != end) { + ForwardIterator temp = begin; + ++begin; + delete *temp; + } +} + +// Variant of STLDeleteContainerPointers which allows the container to +// contain duplicates. +template <class ForwardIterator> +void STLDeleteUniqueContainerPointers(ForwardIterator begin, + ForwardIterator end) { + sort(begin, end); + ForwardIterator new_end = unique(begin, end); + while (begin != new_end) { + ForwardIterator temp = begin; + ++begin; + delete *temp; + } +} + +// STLDeleteElements() deletes all the elements in an STL container and clears +// the container. This function is suitable for use with a vector, set, +// hash_set, or any other STL container which defines sensible begin(), end(), +// and clear() methods. +// +// If container is NULL, this function is a no-op. +// +// As an alternative to calling STLDeleteElements() directly, consider +// ElementDeleter (defined below), which ensures that your container's elements +// are deleted when the ElementDeleter goes out of scope. +template <class T> +void STLDeleteElements(T *container) { + if (!container) return; + STLDeleteContainerPointers(container->begin(), container->end()); + container->clear(); +} + +} // namespace ceres + +#endif // CERES_INTERNAL_STL_UTIL_H_ diff --git a/extern/ceres/internal/ceres/stringprintf.cc b/extern/ceres/internal/ceres/stringprintf.cc new file mode 100644 index 00000000000..d1d8b5fe8ab --- /dev/null +++ b/extern/ceres/internal/ceres/stringprintf.cc @@ -0,0 +1,132 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Sanjay Ghemawat + +#include "ceres/stringprintf.h" + +#include <cerrno> +#include <cstdarg> // For va_list and related operations +#include <cstdio> // MSVC requires this for _vsnprintf +#include <string> +#include <vector> + +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +using std::string; + +#ifdef _MSC_VER +enum { IS_COMPILER_MSVC = 1 }; +#if _MSC_VER < 1800 +#define va_copy(d, s) ((d) = (s)) +#endif +#else +enum { IS_COMPILER_MSVC = 0 }; +#endif + +void StringAppendV(string* dst, const char* format, va_list ap) { + // First try with a small fixed size buffer + char space[1024]; + + // It's possible for methods that use a va_list to invalidate + // the data in it upon use. The fix is to make a copy + // of the structure before using it and use that copy instead. + va_list backup_ap; + va_copy(backup_ap, ap); + int result = vsnprintf(space, sizeof(space), format, backup_ap); + va_end(backup_ap); + + if (result < sizeof(space)) { + if (result >= 0) { + // Normal case -- everything fit. + dst->append(space, result); + return; + } + + if (IS_COMPILER_MSVC) { + // Error or MSVC running out of space. MSVC 8.0 and higher + // can be asked about space needed with the special idiom below: + va_copy(backup_ap, ap); + result = vsnprintf(NULL, 0, format, backup_ap); + va_end(backup_ap); + } + + if (result < 0) { + // Just an error. + return; + } + } + + // Increase the buffer size to the size requested by vsnprintf, + // plus one for the closing \0. + int length = result+1; + char* buf = new char[length]; + + // Restore the va_list before we use it again + va_copy(backup_ap, ap); + result = vsnprintf(buf, length, format, backup_ap); + va_end(backup_ap); + + if (result >= 0 && result < length) { + // It fit + dst->append(buf, result); + } + delete[] buf; +} + + +string StringPrintf(const char* format, ...) { + va_list ap; + va_start(ap, format); + string result; + StringAppendV(&result, format, ap); + va_end(ap); + return result; +} + +const string& SStringPrintf(string* dst, const char* format, ...) { + va_list ap; + va_start(ap, format); + dst->clear(); + StringAppendV(dst, format, ap); + va_end(ap); + return *dst; +} + +void StringAppendF(string* dst, const char* format, ...) { + va_list ap; + va_start(ap, format); + StringAppendV(dst, format, ap); + va_end(ap); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/stringprintf.h b/extern/ceres/internal/ceres/stringprintf.h new file mode 100644 index 00000000000..feeb9c23430 --- /dev/null +++ b/extern/ceres/internal/ceres/stringprintf.h @@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Sanjay Ghemawat +// +// Printf variants that place their output in a C++ string. +// +// Usage: +// string result = StringPrintf("%d %s\n", 10, "hello"); +// SStringPrintf(&result, "%d %s\n", 10, "hello"); +// StringAppendF(&result, "%d %s\n", 20, "there"); + +#ifndef CERES_INTERNAL_STRINGPRINTF_H_ +#define CERES_INTERNAL_STRINGPRINTF_H_ + +#include <cstdarg> +#include <string> + +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +#if (defined(__GNUC__) || defined(__clang__)) +// Tell the compiler to do printf format string checking if the compiler +// supports it; see the 'format' attribute in +// <http://gcc.gnu.org/onlinedocs/gcc-4.3.0/gcc/Function-Attributes.html>. +// +// N.B.: As the GCC manual states, "[s]ince non-static C++ methods +// have an implicit 'this' argument, the arguments of such methods +// should be counted from two, not one." +#define CERES_PRINTF_ATTRIBUTE(string_index, first_to_check) \ + __attribute__((__format__ (__printf__, string_index, first_to_check))) +#define CERES_SCANF_ATTRIBUTE(string_index, first_to_check) \ + __attribute__((__format__ (__scanf__, string_index, first_to_check))) +#else +#define CERES_PRINTF_ATTRIBUTE(string_index, first_to_check) +#endif + +// Return a C++ string. +extern std::string StringPrintf(const char* format, ...) + // Tell the compiler to do printf format string checking. + CERES_PRINTF_ATTRIBUTE(1, 2); + +// Store result into a supplied string and return it. +extern const std::string& SStringPrintf(std::string* dst, const char* format, ...) + // Tell the compiler to do printf format string checking. + CERES_PRINTF_ATTRIBUTE(2, 3); + +// Append result to a supplied string. +extern void StringAppendF(std::string* dst, const char* format, ...) + // Tell the compiler to do printf format string checking. + CERES_PRINTF_ATTRIBUTE(2, 3); + +// Lower-level routine that takes a va_list and appends to a specified string. +// All other routines are just convenience wrappers around it. +extern void StringAppendV(std::string* dst, const char* format, va_list ap); + +#undef CERES_PRINTF_ATTRIBUTE + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_STRINGPRINTF_H_ diff --git a/extern/ceres/internal/ceres/suitesparse.h b/extern/ceres/internal/ceres/suitesparse.h new file mode 100644 index 00000000000..380d76e003a --- /dev/null +++ b/extern/ceres/internal/ceres/suitesparse.h @@ -0,0 +1,306 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A simple C++ interface to the SuiteSparse and CHOLMOD libraries. + +#ifndef CERES_INTERNAL_SUITESPARSE_H_ +#define CERES_INTERNAL_SUITESPARSE_H_ + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_NO_SUITESPARSE + +#include <cstring> +#include <string> +#include <vector> + +#include "ceres/linear_solver.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 { + +class CompressedRowSparseMatrix; +class TripletSparseMatrix; + +// The raw CHOLMOD and SuiteSparseQR libraries have a slightly +// cumbersome c like calling format. This object abstracts it away and +// provides the user with a simpler interface. The methods here cannot +// be static as a cholmod_common object serves as a global variable +// for all cholmod function calls. +class SuiteSparse { + public: + SuiteSparse(); + ~SuiteSparse(); + + // Functions for building cholmod_sparse objects from sparse + // matrices stored in triplet form. The matrix A is not + // modifed. Called owns the result. + cholmod_sparse* CreateSparseMatrix(TripletSparseMatrix* A); + + // This function works like CreateSparseMatrix, except that the + // return value corresponds to A' rather than A. + cholmod_sparse* CreateSparseMatrixTranspose(TripletSparseMatrix* A); + + // Create a cholmod_sparse wrapper around the contents of A. This is + // a shallow object, which refers to the contents of A and does not + // use the SuiteSparse machinery to allocate memory. + cholmod_sparse CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A); + + // Given a vector x, build a cholmod_dense vector of size out_size + // with the first in_size entries copied from x. If x is NULL, then + // an all zeros vector is returned. Caller owns the result. + cholmod_dense* CreateDenseVector(const double* x, int in_size, int out_size); + + // The matrix A is scaled using the matrix whose diagonal is the + // vector scale. mode describes how scaling is applied. Possible + // values are CHOLMOD_ROW for row scaling - diag(scale) * A, + // CHOLMOD_COL for column scaling - A * diag(scale) and CHOLMOD_SYM + // for symmetric scaling which scales both the rows and the columns + // - diag(scale) * A * diag(scale). + void Scale(cholmod_dense* scale, int mode, cholmod_sparse* A) { + cholmod_scale(scale, mode, A, &cc_); + } + + // Create and return a matrix m = A * A'. Caller owns the + // result. The matrix A is not modified. + cholmod_sparse* AATranspose(cholmod_sparse* A) { + cholmod_sparse*m = cholmod_aat(A, NULL, A->nrow, 1, &cc_); + m->stype = 1; // Pay attention to the upper triangular part. + return m; + } + + // y = alpha * A * x + beta * y. Only y is modified. + void SparseDenseMultiply(cholmod_sparse* A, double alpha, double beta, + cholmod_dense* x, cholmod_dense* y) { + double alpha_[2] = {alpha, 0}; + double beta_[2] = {beta, 0}; + cholmod_sdmult(A, 0, alpha_, beta_, x, y, &cc_); + } + + // Find an ordering of A or AA' (if A is unsymmetric) that minimizes + // the fill-in in the Cholesky factorization of the corresponding + // matrix. This is done by using the AMD algorithm. + // + // Using this ordering, the symbolic Cholesky factorization of A (or + // AA') is computed and returned. + // + // A is not modified, only the pattern of non-zeros of A is used, + // the actual numerical values in A are of no consequence. + // + // message contains an explanation of the failures if any. + // + // Caller owns the result. + cholmod_factor* AnalyzeCholesky(cholmod_sparse* A, std::string* message); + + cholmod_factor* BlockAnalyzeCholesky(cholmod_sparse* A, + const std::vector<int>& row_blocks, + const std::vector<int>& col_blocks, + std::string* message); + + // If A is symmetric, then compute the symbolic Cholesky + // factorization of A(ordering, ordering). If A is unsymmetric, then + // compute the symbolic factorization of + // A(ordering,:) A(ordering,:)'. + // + // A is not modified, only the pattern of non-zeros of A is used, + // the actual numerical values in A are of no consequence. + // + // message contains an explanation of the failures if any. + // + // Caller owns the result. + cholmod_factor* AnalyzeCholeskyWithUserOrdering( + cholmod_sparse* A, + const std::vector<int>& ordering, + std::string* message); + + // Perform a symbolic factorization of A without re-ordering A. No + // postordering of the elimination tree is performed. This ensures + // that the symbolic factor does not introduce an extra permutation + // on the matrix. See the documentation for CHOLMOD for more details. + // + // message contains an explanation of the failures if any. + cholmod_factor* AnalyzeCholeskyWithNaturalOrdering(cholmod_sparse* A, + std::string* message); + + // Use the symbolic factorization in L, to find the numerical + // factorization for the matrix A or AA^T. Return true if + // successful, false otherwise. L contains the numeric factorization + // on return. + // + // message contains an explanation of the failures if any. + LinearSolverTerminationType Cholesky(cholmod_sparse* A, + cholmod_factor* L, + std::string* message); + + // Given a Cholesky factorization of a matrix A = LL^T, solve the + // linear system Ax = b, and return the result. If the Solve fails + // NULL is returned. Caller owns the result. + // + // message contains an explanation of the failures if any. + cholmod_dense* Solve(cholmod_factor* L, cholmod_dense* b, std::string* message); + + // By virtue of the modeling layer in Ceres being block oriented, + // all the matrices used by Ceres are also block oriented. When + // doing sparse direct factorization of these matrices the + // fill-reducing ordering algorithms (in particular AMD) can either + // be run on the block or the scalar form of these matrices. The two + // SuiteSparse::AnalyzeCholesky methods allows the the client to + // compute the symbolic factorization of a matrix by either using + // AMD on the matrix or a user provided ordering of the rows. + // + // But since the underlying matrices are block oriented, it is worth + // running AMD on just the block structre of these matrices and then + // lifting these block orderings to a full scalar ordering. This + // preserves the block structure of the permuted matrix, and exposes + // more of the super-nodal structure of the matrix to the numerical + // factorization routines. + // + // Find the block oriented AMD ordering of a matrix A, whose row and + // column blocks are given by row_blocks, and col_blocks + // respectively. The matrix may or may not be symmetric. The entries + // of col_blocks do not need to sum to the number of columns in + // A. If this is the case, only the first sum(col_blocks) are used + // to compute the ordering. + bool BlockAMDOrdering(const cholmod_sparse* A, + const std::vector<int>& row_blocks, + const std::vector<int>& col_blocks, + std::vector<int>* ordering); + + // Find a fill reducing approximate minimum degree + // ordering. ordering is expected to be large enough to hold the + // ordering. + bool 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. + bool 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_); } + + void Print(cholmod_sparse* m, const std::string& name) { + cholmod_print_sparse(m, const_cast<char*>(name.c_str()), &cc_); + } + + void Print(cholmod_dense* m, const std::string& name) { + cholmod_print_dense(m, const_cast<char*>(name.c_str()), &cc_); + } + + void Print(cholmod_triplet* m, const std::string& name) { + cholmod_print_triplet(m, const_cast<char*>(name.c_str()), &cc_); + } + + cholmod_common* mutable_cc() { return &cc_; } + + private: + cholmod_common cc_; +}; + +} // namespace internal +} // namespace ceres + +#else // CERES_NO_SUITESPARSE + +typedef void cholmod_factor; + +class SuiteSparse { + public: + // Defining this static function even when SuiteSparse is not + // available, allows client code to check for the presence of CAMD + // without checking for the absence of the CERES_NO_CAMD symbol. + // + // This is safer because the symbol maybe missing due to a user + // accidently not including suitesparse.h in their code when + // checking for the symbol. + static bool IsConstrainedApproximateMinimumDegreeOrderingAvailable() { + return false; + } + + void Free(void* arg) {} +}; + +#endif // CERES_NO_SUITESPARSE + +#endif // CERES_INTERNAL_SUITESPARSE_H_ diff --git a/extern/ceres/internal/ceres/triplet_sparse_matrix.cc b/extern/ceres/internal/ceres/triplet_sparse_matrix.cc new file mode 100644 index 00000000000..8df405ca115 --- /dev/null +++ b/extern/ceres/internal/ceres/triplet_sparse_matrix.cc @@ -0,0 +1,264 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/triplet_sparse_matrix.h" + +#include <algorithm> +#include <cstddef> +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +TripletSparseMatrix::TripletSparseMatrix() + : num_rows_(0), + num_cols_(0), + max_num_nonzeros_(0), + num_nonzeros_(0), + rows_(NULL), + cols_(NULL), + values_(NULL) {} + +TripletSparseMatrix::~TripletSparseMatrix() {} + +TripletSparseMatrix::TripletSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros) + : num_rows_(num_rows), + num_cols_(num_cols), + max_num_nonzeros_(max_num_nonzeros), + num_nonzeros_(0), + rows_(NULL), + cols_(NULL), + values_(NULL) { + // All the sizes should at least be zero + CHECK_GE(num_rows, 0); + CHECK_GE(num_cols, 0); + CHECK_GE(max_num_nonzeros, 0); + AllocateMemory(); +} + +TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig) + : SparseMatrix(), + num_rows_(orig.num_rows_), + num_cols_(orig.num_cols_), + max_num_nonzeros_(orig.max_num_nonzeros_), + num_nonzeros_(orig.num_nonzeros_), + rows_(NULL), + cols_(NULL), + values_(NULL) { + AllocateMemory(); + CopyData(orig); +} + +TripletSparseMatrix& TripletSparseMatrix::operator=( + const TripletSparseMatrix& rhs) { + num_rows_ = rhs.num_rows_; + num_cols_ = rhs.num_cols_; + num_nonzeros_ = rhs.num_nonzeros_; + max_num_nonzeros_ = rhs.max_num_nonzeros_; + AllocateMemory(); + CopyData(rhs); + return *this; +} + +bool TripletSparseMatrix::AllTripletsWithinBounds() const { + for (int i = 0; i < num_nonzeros_; ++i) { + if ((rows_[i] < 0) || (rows_[i] >= num_rows_) || + (cols_[i] < 0) || (cols_[i] >= num_cols_)) + return false; + } + return true; +} + +void TripletSparseMatrix::Reserve(int new_max_num_nonzeros) { + CHECK_LE(num_nonzeros_, new_max_num_nonzeros) + << "Reallocation will cause data loss"; + + // Nothing to do if we have enough space already. + if (new_max_num_nonzeros <= max_num_nonzeros_) + return; + + int* new_rows = new int[new_max_num_nonzeros]; + int* new_cols = new int[new_max_num_nonzeros]; + double* new_values = new double[new_max_num_nonzeros]; + + for (int i = 0; i < num_nonzeros_; ++i) { + new_rows[i] = rows_[i]; + new_cols[i] = cols_[i]; + new_values[i] = values_[i]; + } + + rows_.reset(new_rows); + cols_.reset(new_cols); + values_.reset(new_values); + + max_num_nonzeros_ = new_max_num_nonzeros; +} + +void TripletSparseMatrix::SetZero() { + std::fill(values_.get(), values_.get() + max_num_nonzeros_, 0.0); + num_nonzeros_ = 0; +} + +void TripletSparseMatrix::set_num_nonzeros(int num_nonzeros) { + CHECK_GE(num_nonzeros, 0); + CHECK_LE(num_nonzeros, max_num_nonzeros_); + num_nonzeros_ = num_nonzeros; +} + +void TripletSparseMatrix::AllocateMemory() { + rows_.reset(new int[max_num_nonzeros_]); + cols_.reset(new int[max_num_nonzeros_]); + values_.reset(new double[max_num_nonzeros_]); +} + +void TripletSparseMatrix::CopyData(const TripletSparseMatrix& orig) { + for (int i = 0; i < num_nonzeros_; ++i) { + rows_[i] = orig.rows_[i]; + cols_[i] = orig.cols_[i]; + values_[i] = orig.values_[i]; + } +} + +void TripletSparseMatrix::RightMultiply(const double* x, double* y) const { + for (int i = 0; i < num_nonzeros_; ++i) { + y[rows_[i]] += values_[i]*x[cols_[i]]; + } +} + +void TripletSparseMatrix::LeftMultiply(const double* x, double* y) const { + for (int i = 0; i < num_nonzeros_; ++i) { + y[cols_[i]] += values_[i]*x[rows_[i]]; + } +} + +void TripletSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + VectorRef(x, num_cols_).setZero(); + for (int i = 0; i < num_nonzeros_; ++i) { + x[cols_[i]] += values_[i] * values_[i]; + } +} + +void TripletSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + for (int i = 0; i < num_nonzeros_; ++i) { + values_[i] = values_[i] * scale[cols_[i]]; + } +} + +void TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + Matrix& m = *dense_matrix; + for (int i = 0; i < num_nonzeros_; ++i) { + m(rows_[i], cols_[i]) += values_[i]; + } +} + +void TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) { + CHECK_EQ(B.num_cols(), num_cols_); + Reserve(num_nonzeros_ + B.num_nonzeros_); + for (int i = 0; i < B.num_nonzeros_; ++i) { + rows_.get()[num_nonzeros_] = B.rows()[i] + num_rows_; + cols_.get()[num_nonzeros_] = B.cols()[i]; + values_.get()[num_nonzeros_++] = B.values()[i]; + } + num_rows_ = num_rows_ + B.num_rows(); +} + +void TripletSparseMatrix::AppendCols(const TripletSparseMatrix& B) { + CHECK_EQ(B.num_rows(), num_rows_); + Reserve(num_nonzeros_ + B.num_nonzeros_); + for (int i = 0; i < B.num_nonzeros_; ++i, ++num_nonzeros_) { + rows_.get()[num_nonzeros_] = B.rows()[i]; + cols_.get()[num_nonzeros_] = B.cols()[i] + num_cols_; + values_.get()[num_nonzeros_] = B.values()[i]; + } + num_cols_ = num_cols_ + B.num_cols(); +} + + +void TripletSparseMatrix::Resize(int new_num_rows, int new_num_cols) { + if ((new_num_rows >= num_rows_) && (new_num_cols >= num_cols_)) { + num_rows_ = new_num_rows; + num_cols_ = new_num_cols; + return; + } + + num_rows_ = new_num_rows; + num_cols_ = new_num_cols; + + int* r_ptr = rows_.get(); + int* c_ptr = cols_.get(); + double* v_ptr = values_.get(); + + int dropped_terms = 0; + for (int i = 0; i < num_nonzeros_; ++i) { + if ((r_ptr[i] < num_rows_) && (c_ptr[i] < num_cols_)) { + if (dropped_terms) { + r_ptr[i-dropped_terms] = r_ptr[i]; + c_ptr[i-dropped_terms] = c_ptr[i]; + v_ptr[i-dropped_terms] = v_ptr[i]; + } + } else { + ++dropped_terms; + } + } + num_nonzeros_ -= dropped_terms; +} + +TripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix( + const double* values, int num_rows) { + TripletSparseMatrix* m = + new TripletSparseMatrix(num_rows, num_rows, num_rows); + for (int i = 0; i < num_rows; ++i) { + m->mutable_rows()[i] = i; + m->mutable_cols()[i] = i; + m->mutable_values()[i] = values[i]; + } + m->set_num_nonzeros(num_rows); + return m; +} + +void TripletSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + for (int i = 0; i < num_nonzeros_; ++i) { + fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/triplet_sparse_matrix.h b/extern/ceres/internal/ceres/triplet_sparse_matrix.h new file mode 100644 index 00000000000..f3f5370df6f --- /dev/null +++ b/extern/ceres/internal/ceres/triplet_sparse_matrix.h @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H_ + +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// An implementation of the SparseMatrix interface to store and +// manipulate sparse matrices in triplet (i,j,s) form. This object is +// inspired by the design of the cholmod_triplet struct used in the +// SuiteSparse package and is memory layout compatible with it. +class TripletSparseMatrix : public SparseMatrix { + public: + TripletSparseMatrix(); + TripletSparseMatrix(int num_rows, int num_cols, int max_num_nonzeros); + explicit TripletSparseMatrix(const TripletSparseMatrix& orig); + + TripletSparseMatrix& operator=(const TripletSparseMatrix& rhs); + + ~TripletSparseMatrix(); + + // Implementation of the SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; + virtual void ToTextFile(FILE* file) const; + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_cols_; } + virtual int num_nonzeros() const { return num_nonzeros_; } + virtual const double* values() const { return values_.get(); } + virtual double* mutable_values() { return values_.get(); } + virtual void set_num_nonzeros(int num_nonzeros); + + // Increase max_num_nonzeros and correspondingly increase the size + // of rows_, cols_ and values_. If new_max_num_nonzeros is smaller + // than max_num_nonzeros_, then num_non_zeros should be less than or + // equal to new_max_num_nonzeros, otherwise data loss is possible + // and the method crashes. + void Reserve(int new_max_num_nonzeros); + + // Append the matrix B at the bottom of this matrix. B should have + // the same number of columns as num_cols_. + void AppendRows(const TripletSparseMatrix& B); + + // Append the matrix B at the right of this matrix. B should have + // the same number of rows as num_rows_; + void AppendCols(const TripletSparseMatrix& B); + + // Resize the matrix. Entries which fall outside the new matrix + // bounds are dropped and the num_non_zeros changed accordingly. + void Resize(int new_num_rows, int new_num_cols); + + int max_num_nonzeros() const { return max_num_nonzeros_; } + const int* rows() const { return rows_.get(); } + const int* cols() const { return cols_.get(); } + int* mutable_rows() { return rows_.get(); } + int* mutable_cols() { return cols_.get(); } + + // Returns true if the entries of the matrix obey the row, column, + // and column size bounds and false otherwise. + bool AllTripletsWithinBounds() const; + + bool IsValid() const { return AllTripletsWithinBounds(); } + + // Build a sparse diagonal matrix of size num_rows x num_rows from + // the array values. Entries of the values array are copied into the + // sparse matrix. + static TripletSparseMatrix* CreateSparseDiagonalMatrix(const double* values, + int num_rows); + + private: + void AllocateMemory(); + void CopyData(const TripletSparseMatrix& orig); + + int num_rows_; + int num_cols_; + int max_num_nonzeros_; + int num_nonzeros_; + + // The data is stored as three arrays. For each i, values_[i] is + // stored at the location (rows_[i], cols_[i]). If the there are + // multiple entries with the same (rows_[i], cols_[i]), the values_ + // entries corresponding to them are summed up. + scoped_array<int> rows_; + scoped_array<int> cols_; + scoped_array<double> values_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H__ diff --git a/extern/ceres/internal/ceres/trust_region_minimizer.cc b/extern/ceres/internal/ceres/trust_region_minimizer.cc new file mode 100644 index 00000000000..d654d0867f1 --- /dev/null +++ b/extern/ceres/internal/ceres/trust_region_minimizer.cc @@ -0,0 +1,716 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/trust_region_minimizer.h" + +#include <algorithm> +#include <cmath> +#include <cstdlib> +#include <cstring> +#include <limits> +#include <string> +#include <vector> + +#include "Eigen/Core" +#include "ceres/array_utils.h" +#include "ceres/coordinate_descent_minimizer.h" +#include "ceres/evaluator.h" +#include "ceres/file.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/line_search.h" +#include "ceres/linear_least_squares_problems.h" +#include "ceres/sparse_matrix.h" +#include "ceres/stringprintf.h" +#include "ceres/trust_region_strategy.h" +#include "ceres/types.h" +#include "ceres/wall_time.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { + +LineSearch::Summary DoLineSearch(const Minimizer::Options& options, + const Vector& x, + const Vector& gradient, + const double cost, + const Vector& delta, + Evaluator* evaluator) { + LineSearchFunction line_search_function(evaluator); + + LineSearch::Options line_search_options; + line_search_options.is_silent = true; + 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; + + std::string message; + scoped_ptr<LineSearch> line_search( + CHECK_NOTNULL(LineSearch::Create(ceres::ARMIJO, + line_search_options, + &message))); + LineSearch::Summary summary; + line_search_function.Init(x, delta); + line_search->Search(1.0, cost, gradient.dot(delta), &summary); + return summary; +} + +} // namespace + +// Compute a scaling vector that is used to improve the conditioning +// of the Jacobian. +void TrustRegionMinimizer::EstimateScale(const SparseMatrix& jacobian, + double* scale) const { + jacobian.SquaredColumnNorm(scale); + for (int i = 0; i < jacobian.num_cols(); ++i) { + scale[i] = 1.0 / (1.0 + sqrt(scale[i])); + } +} + +void TrustRegionMinimizer::Init(const Minimizer::Options& options) { + options_ = options; + sort(options_.trust_region_minimizer_iterations_to_dump.begin(), + options_.trust_region_minimizer_iterations_to_dump.end()); +} + +void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary) { + double start_time = WallTimeInSeconds(); + double iteration_start_time = start_time; + Init(options); + + Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator.get()); + SparseMatrix* jacobian = CHECK_NOTNULL(options_.jacobian.get()); + TrustRegionStrategy* strategy = + CHECK_NOTNULL(options_.trust_region_strategy.get()); + + const bool is_not_silent = !options.is_silent; + + // If the problem is bounds constrained, then enable the use of a + // line search after the trust region step has been computed. This + // line search will automatically use a projected test point onto + // the feasible set, there by guaranteeing the feasibility of the + // final output. + // + // TODO(sameeragarwal): Make line search available more generally. + const bool use_line_search = options.is_constrained; + + summary->termination_type = NO_CONVERGENCE; + summary->num_successful_steps = 0; + summary->num_unsuccessful_steps = 0; + summary->is_constrained = options.is_constrained; + + const int num_parameters = evaluator->NumParameters(); + const int num_effective_parameters = evaluator->NumEffectiveParameters(); + const int num_residuals = evaluator->NumResiduals(); + + Vector residuals(num_residuals); + Vector trust_region_step(num_effective_parameters); + Vector delta(num_effective_parameters); + Vector x_plus_delta(num_parameters); + Vector gradient(num_effective_parameters); + Vector model_residuals(num_residuals); + Vector scale(num_effective_parameters); + Vector negative_gradient(num_effective_parameters); + Vector projected_gradient_step(num_parameters); + + IterationSummary iteration_summary; + iteration_summary.iteration = 0; + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; + iteration_summary.cost_change = 0.0; + iteration_summary.gradient_max_norm = 0.0; + iteration_summary.gradient_norm = 0.0; + iteration_summary.step_norm = 0.0; + iteration_summary.relative_decrease = 0.0; + iteration_summary.trust_region_radius = strategy->Radius(); + iteration_summary.eta = options_.eta; + iteration_summary.linear_solver_iterations = 0; + iteration_summary.step_solver_time_in_seconds = 0; + + VectorRef x_min(parameters, num_parameters); + Vector x = x_min; + // Project onto the feasible set. + if (options.is_constrained) { + delta.setZero(); + if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) { + summary->message = + "Unable to project initial point onto the feasible set."; + summary->termination_type = FAILURE; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + return; + } + x_min = x_plus_delta; + x = x_plus_delta; + } + + double x_norm = x.norm(); + + // Do initial cost and Jacobian evaluation. + double cost = 0.0; + if (!evaluator->Evaluate(x.data(), + &cost, + residuals.data(), + gradient.data(), + jacobian)) { + summary->message = "Residual and Jacobian evaluation failed."; + summary->termination_type = FAILURE; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + return; + } + + negative_gradient = -gradient; + if (!evaluator->Plus(x.data(), + negative_gradient.data(), + projected_gradient_step.data())) { + summary->message = "Unable to compute gradient step."; + summary->termination_type = FAILURE; + LOG(ERROR) << "Terminating: " << summary->message; + return; + } + + summary->initial_cost = cost + summary->fixed_cost; + iteration_summary.cost = cost + summary->fixed_cost; + iteration_summary.gradient_max_norm = + (x - projected_gradient_step).lpNorm<Eigen::Infinity>(); + iteration_summary.gradient_norm = (x - projected_gradient_step).norm(); + + if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) { + summary->message = StringPrintf("Gradient tolerance reached. " + "Gradient max norm: %e <= %e", + iteration_summary.gradient_max_norm, + options_.gradient_tolerance); + summary->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + + // Ensure that there is an iteration summary object for iteration + // 0 in Summary::iterations. + iteration_summary.iteration_time_in_seconds = + WallTimeInSeconds() - iteration_start_time; + iteration_summary.cumulative_time_in_seconds = + WallTimeInSeconds() - start_time + + summary->preprocessor_time_in_seconds; + summary->iterations.push_back(iteration_summary); + return; + } + + if (options_.jacobi_scaling) { + EstimateScale(*jacobian, scale.data()); + jacobian->ScaleColumns(scale.data()); + } else { + scale.setOnes(); + } + + iteration_summary.iteration_time_in_seconds = + WallTimeInSeconds() - iteration_start_time; + iteration_summary.cumulative_time_in_seconds = + WallTimeInSeconds() - start_time + + summary->preprocessor_time_in_seconds; + summary->iterations.push_back(iteration_summary); + + int num_consecutive_nonmonotonic_steps = 0; + double minimum_cost = cost; + double reference_cost = cost; + double accumulated_reference_model_cost_change = 0.0; + double candidate_cost = cost; + double accumulated_candidate_model_cost_change = 0.0; + int num_consecutive_invalid_steps = 0; + bool inner_iterations_are_enabled = + options.inner_iteration_minimizer.get() != NULL; + while (true) { + bool inner_iterations_were_useful = false; + if (!RunCallbacks(options, iteration_summary, summary)) { + return; + } + + iteration_start_time = WallTimeInSeconds(); + if (iteration_summary.iteration >= options_.max_num_iterations) { + summary->message = "Maximum number of iterations reached."; + summary->termination_type = NO_CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + return; + } + + const double total_solver_time = iteration_start_time - start_time + + summary->preprocessor_time_in_seconds; + if (total_solver_time >= options_.max_solver_time_in_seconds) { + summary->message = "Maximum solver time reached."; + summary->termination_type = NO_CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + return; + } + + 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()); + + if (strategy_summary.termination_type == LINEAR_SOLVER_FATAL_ERROR) { + summary->message = + "Linear solver failed due to unrecoverable " + "non-numeric causes. Please see the error log for clues. "; + summary->termination_type = FAILURE; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + return; + } + + 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; + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; + + double model_cost_change = 0.0; + if (strategy_summary.termination_type != LINEAR_SOLVER_FAILURE) { + // new_model_cost + // = 1/2 [f + J * step]^2 + // = 1/2 [ f'f + 2f'J * step + step' * J' * J * step ] + // model_cost_change + // = cost - new_model_cost + // = f'f/2 - 1/2 [ f'f + 2f'J * step + step' * J' * J * step] + // = -f'J * step - step' * J' * J * step / 2 + model_residuals.setZero(); + jacobian->RightMultiply(trust_region_step.data(), model_residuals.data()); + model_cost_change = + - model_residuals.dot(residuals + model_residuals / 2.0); + + if (model_cost_change < 0.0) { + VLOG_IF(1, is_not_silent) + << "Invalid step: current_cost: " << cost + << " absolute difference " << model_cost_change + << " relative difference " << (model_cost_change / cost); + } else { + iteration_summary.step_is_valid = true; + } + } + + if (!iteration_summary.step_is_valid) { + // Invalid steps can happen due to a number of reasons, and we + // allow a limited number of successive failures, and return with + // FAILURE if this limit is exceeded. + if (++num_consecutive_invalid_steps >= + options_.max_num_consecutive_invalid_steps) { + summary->message = StringPrintf( + "Number of successive invalid steps more " + "than Solver::Options::max_num_consecutive_invalid_steps: %d", + options_.max_num_consecutive_invalid_steps); + summary->termination_type = FAILURE; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + return; + } + + // We are going to try and reduce the trust region radius and + // solve again. To do this, we are going to treat this iteration + // as an unsuccessful iteration. Since the various callbacks are + // still executed, we are going to fill the iteration summary + // with data that assumes a step of length zero and no progress. + iteration_summary.cost = cost + summary->fixed_cost; + iteration_summary.cost_change = 0.0; + iteration_summary.gradient_max_norm = + summary->iterations.back().gradient_max_norm; + iteration_summary.gradient_norm = + summary->iterations.back().gradient_norm; + iteration_summary.step_norm = 0.0; + iteration_summary.relative_decrease = 0.0; + iteration_summary.eta = options_.eta; + } else { + // The step is numerically valid, so now we can judge its quality. + num_consecutive_invalid_steps = 0; + + // Undo the Jacobian column scaling. + delta = (trust_region_step.array() * scale.array()).matrix(); + + // Try improving the step further by using an ARMIJO line + // search. + // + // TODO(sameeragarwal): What happens to trust region sizing as + // it interacts with the line search ? + if (use_line_search) { + const LineSearch::Summary line_search_summary = + DoLineSearch(options, x, gradient, cost, delta, evaluator); + + summary->line_search_cost_evaluation_time_in_seconds += + line_search_summary.cost_evaluation_time_in_seconds; + summary->line_search_gradient_evaluation_time_in_seconds += + line_search_summary.gradient_evaluation_time_in_seconds; + summary->line_search_polynomial_minimization_time_in_seconds += + line_search_summary.polynomial_minimization_time_in_seconds; + summary->line_search_total_time_in_seconds += + line_search_summary.total_time_in_seconds; + + if (line_search_summary.success) { + delta *= line_search_summary.optimal_step_size; + } + } + + double new_cost = std::numeric_limits<double>::max(); + if (evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) { + if (!evaluator->Evaluate(x_plus_delta.data(), + &new_cost, + NULL, + NULL, + NULL)) { + LOG_IF(WARNING, is_not_silent) + << "Step failed to evaluate. " + << "Treating it as a step with infinite cost"; + new_cost = std::numeric_limits<double>::max(); + } + } else { + LOG_IF(WARNING, is_not_silent) + << "x_plus_delta = Plus(x, delta) failed. " + << "Treating it as a step with infinite cost"; + } + + if (new_cost < std::numeric_limits<double>::max()) { + // Check if performing an inner iteration will make it better. + 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; + options.inner_iteration_minimizer->Minimize(options, + inner_iteration_x.data(), + &inner_iteration_summary); + if (!evaluator->Evaluate(inner_iteration_x.data(), + &new_cost, + NULL, NULL, NULL)) { + VLOG_IF(2, is_not_silent) << "Inner iteration failed."; + new_cost = x_plus_delta_cost; + } else { + x_plus_delta = inner_iteration_x; + // Boost the model_cost_change, since the inner iteration + // improvements are not accounted for by the trust region. + model_cost_change += x_plus_delta_cost - new_cost; + VLOG_IF(2, is_not_silent) + << "Inner iteration succeeded; Current cost: " << cost + << " Trust region step cost: " << x_plus_delta_cost + << " Inner iteration cost: " << new_cost; + + inner_iterations_were_useful = new_cost < cost; + + const double inner_iteration_relative_progress = + 1.0 - new_cost / x_plus_delta_cost; + // Disable inner iterations once the relative improvement + // drops below tolerance. + inner_iterations_are_enabled = + (inner_iteration_relative_progress > + options.inner_iteration_tolerance); + VLOG_IF(2, is_not_silent && !inner_iterations_are_enabled) + << "Disabling inner iterations. Progress : " + << inner_iteration_relative_progress; + } + summary->inner_iteration_time_in_seconds += + WallTimeInSeconds() - inner_iteration_start_time; + } + } + + iteration_summary.step_norm = (x - x_plus_delta).norm(); + + // Convergence based on parameter_tolerance. + const double step_size_tolerance = options_.parameter_tolerance * + (x_norm + options_.parameter_tolerance); + if (iteration_summary.step_norm <= step_size_tolerance) { + summary->message = + StringPrintf("Parameter tolerance reached. " + "Relative step_norm: %e <= %e.", + (iteration_summary.step_norm / + (x_norm + options_.parameter_tolerance)), + options_.parameter_tolerance); + summary->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + return; + } + + iteration_summary.cost_change = cost - new_cost; + const double absolute_function_tolerance = + options_.function_tolerance * cost; + if (fabs(iteration_summary.cost_change) <= absolute_function_tolerance) { + summary->message = + StringPrintf("Function tolerance reached. " + "|cost_change|/cost: %e <= %e", + fabs(iteration_summary.cost_change) / cost, + options_.function_tolerance); + summary->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + return; + } + + const double relative_decrease = + iteration_summary.cost_change / model_cost_change; + + const double historical_relative_decrease = + (reference_cost - new_cost) / + (accumulated_reference_model_cost_change + model_cost_change); + + // If monotonic steps are being used, then the relative_decrease + // is the usual ratio of the change in objective function value + // divided by the change in model cost. + // + // If non-monotonic steps are allowed, then we take the maximum + // of the relative_decrease and the + // historical_relative_decrease, which measures the increase + // from a reference iteration. The model cost change is + // estimated by accumulating the model cost changes since the + // reference iteration. The historical relative_decrease offers + // a boost to a step which is not too bad compared to the + // reference iteration, allowing for non-monotonic steps. + iteration_summary.relative_decrease = + options.use_nonmonotonic_steps + ? std::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 = + (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 (!inner_iterations_were_useful && + relative_decrease <= options_.min_relative_decrease) { + iteration_summary.step_is_nonmonotonic = true; + VLOG_IF(2, is_not_silent) + << "Non-monotonic step! " + << " relative_decrease: " + << relative_decrease + << " historical_relative_decrease: " + << historical_relative_decrease; + } + } + } + + if (iteration_summary.step_is_successful) { + ++summary->num_successful_steps; + strategy->StepAccepted(iteration_summary.relative_decrease); + + x = x_plus_delta; + x_norm = x.norm(); + + // Step looks good, evaluate the residuals and Jacobian at this + // point. + if (!evaluator->Evaluate(x.data(), + &cost, + residuals.data(), + gradient.data(), + jacobian)) { + summary->message = "Residual and Jacobian evaluation failed."; + summary->termination_type = FAILURE; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + return; + } + + negative_gradient = -gradient; + if (!evaluator->Plus(x.data(), + negative_gradient.data(), + projected_gradient_step.data())) { + summary->message = + "projected_gradient_step = Plus(x, -gradient) failed."; + summary->termination_type = FAILURE; + LOG(ERROR) << "Terminating: " << summary->message; + return; + } + + iteration_summary.gradient_max_norm = + (x - projected_gradient_step).lpNorm<Eigen::Infinity>(); + iteration_summary.gradient_norm = (x - projected_gradient_step).norm(); + + if (options_.jacobi_scaling) { + jacobian->ScaleColumns(scale.data()); + } + + // Update the best, reference and candidate iterates. + // + // Based on algorithm 10.1.2 (page 357) of "Trust Region + // Methods" by Conn Gould & Toint, or equations 33-40 of + // "Non-monotone trust-region algorithms for nonlinear + // optimization subject to convex constraints" by Phil Toint, + // Mathematical Programming, 77, 1997. + if (cost < minimum_cost) { + // A step that improves solution quality was found. + x_min = x; + minimum_cost = cost; + // Set the candidate iterate to the current point. + candidate_cost = cost; + num_consecutive_nonmonotonic_steps = 0; + accumulated_candidate_model_cost_change = 0.0; + } else { + ++num_consecutive_nonmonotonic_steps; + if (cost > candidate_cost) { + // The current iterate is has a higher cost than the + // candidate iterate. Set the candidate to this point. + VLOG_IF(2, is_not_silent) + << "Updating the candidate iterate to the current point."; + candidate_cost = cost; + accumulated_candidate_model_cost_change = 0.0; + } + + // At this point we have made too many non-monotonic steps and + // we are going to reset the value of the reference iterate so + // as to force the algorithm to descend. + // + // This is the case because the candidate iterate has a value + // greater than minimum_cost but smaller than the reference + // iterate. + if (num_consecutive_nonmonotonic_steps == + options.max_consecutive_nonmonotonic_steps) { + VLOG_IF(2, is_not_silent) + << "Resetting the reference point to the candidate point"; + reference_cost = candidate_cost; + accumulated_reference_model_cost_change = + accumulated_candidate_model_cost_change; + } + } + } else { + ++summary->num_unsuccessful_steps; + if (iteration_summary.step_is_valid) { + strategy->StepRejected(iteration_summary.relative_decrease); + } else { + strategy->StepIsInvalid(); + } + } + + iteration_summary.cost = cost + summary->fixed_cost; + iteration_summary.trust_region_radius = strategy->Radius(); + iteration_summary.iteration_time_in_seconds = + WallTimeInSeconds() - iteration_start_time; + iteration_summary.cumulative_time_in_seconds = + WallTimeInSeconds() - start_time + + summary->preprocessor_time_in_seconds; + summary->iterations.push_back(iteration_summary); + + // If the step was successful, check for the gradient norm + // collapsing to zero, and if the step is unsuccessful then check + // if the trust region radius has collapsed to zero. + // + // For correctness (Number of IterationSummary objects, correct + // final cost, and state update) these convergence tests need to + // be performed at the end of the iteration. + if (iteration_summary.step_is_successful) { + // Gradient norm can only go down in successful steps. + if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) { + summary->message = StringPrintf("Gradient tolerance reached. " + "Gradient max norm: %e <= %e", + iteration_summary.gradient_max_norm, + options_.gradient_tolerance); + summary->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; + return; + } + } else { + // Trust region radius can only go down if the step if + // unsuccessful. + if (iteration_summary.trust_region_radius < + options_.min_trust_region_radius) { + summary->message = "Termination. Minimum trust region radius reached."; + summary->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent) << summary->message; + return; + } + } + } +} + + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/trust_region_minimizer.h b/extern/ceres/internal/ceres/trust_region_minimizer.h new file mode 100644 index 00000000000..ed52c2642d1 --- /dev/null +++ b/extern/ceres/internal/ceres/trust_region_minimizer.h @@ -0,0 +1,65 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ +#define CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ + +#include "ceres/minimizer.h" +#include "ceres/solver.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// Generic trust region minimization algorithm. The heavy lifting is +// done by a TrustRegionStrategy object passed in as part of options. +// +// For example usage, see SolverImpl::Minimize. +class TrustRegionMinimizer : public Minimizer { + public: + ~TrustRegionMinimizer() {} + virtual void Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary); + + private: + void Init(const Minimizer::Options& options); + void EstimateScale(const SparseMatrix& jacobian, double* scale) const; + bool MaybeDumpLinearLeastSquaresProblem(const int iteration, + const SparseMatrix* jacobian, + const double* residuals, + const double* step) const; + + Minimizer::Options options_; +}; + +} // namespace internal +} // namespace ceres +#endif // CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ diff --git a/extern/ceres/internal/ceres/trust_region_preprocessor.cc b/extern/ceres/internal/ceres/trust_region_preprocessor.cc new file mode 100644 index 00000000000..4020e4ca115 --- /dev/null +++ b/extern/ceres/internal/ceres/trust_region_preprocessor.cc @@ -0,0 +1,362 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/trust_region_preprocessor.h" + +#include <numeric> +#include <string> +#include "ceres/callbacks.h" +#include "ceres/evaluator.h" +#include "ceres/linear_solver.h" +#include "ceres/minimizer.h" +#include "ceres/parameter_block.h" +#include "ceres/preconditioner.h" +#include "ceres/preprocessor.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/reorder_program.h" +#include "ceres/suitesparse.h" +#include "ceres/trust_region_strategy.h" +#include "ceres/wall_time.h" + +namespace ceres { +namespace internal { + +using std::vector; + +namespace { + +ParameterBlockOrdering* CreateDefaultLinearSolverOrdering( + const Program& program) { + ParameterBlockOrdering* ordering = new ParameterBlockOrdering; + const vector<ParameterBlock*>& parameter_blocks = + program.parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + ordering->AddElementToGroup( + const_cast<double*>(parameter_blocks[i]->user_state()), 0); + } + return ordering; +} + +// Check if all the user supplied values in the parameter blocks are +// sane or not, and if the program is feasible or not. +bool IsProgramValid(const Program& program, std::string* error) { + return (program.ParameterBlocksAreFinite(error) && + program.IsFeasible(error)); +} + +void AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver( + Solver::Options* options) { + if (!IsSchurType(options->linear_solver_type)) { + return; + } + + const LinearSolverType linear_solver_type_given = options->linear_solver_type; + const PreconditionerType preconditioner_type_given = + options->preconditioner_type; + options->linear_solver_type = LinearSolver::LinearSolverForZeroEBlocks( + linear_solver_type_given); + + std::string message; + if (linear_solver_type_given == ITERATIVE_SCHUR) { + options->preconditioner_type = Preconditioner::PreconditionerForZeroEBlocks( + preconditioner_type_given); + + message = + StringPrintf( + "No E blocks. Switching from %s(%s) to %s(%s).", + LinearSolverTypeToString(linear_solver_type_given), + PreconditionerTypeToString(preconditioner_type_given), + LinearSolverTypeToString(options->linear_solver_type), + PreconditionerTypeToString(options->preconditioner_type)); + } else { + message = + StringPrintf( + "No E blocks. Switching from %s to %s.", + LinearSolverTypeToString(linear_solver_type_given), + LinearSolverTypeToString(options->linear_solver_type)); + } + + VLOG_IF(1, options->logging_type != SILENT) << message; +} + +// For Schur type and SPARSE_NORMAL_CHOLESKY linear solvers, reorder +// the program to reduce fill-in and increase cache coherency. +bool ReorderProgram(PreprocessedProblem* pp) { + Solver::Options& options = pp->options; + if (IsSchurType(options.linear_solver_type)) { + return ReorderProgramForSchurTypeLinearSolver( + options.linear_solver_type, + options.sparse_linear_algebra_library_type, + pp->problem->parameter_map(), + options.linear_solver_ordering.get(), + pp->reduced_program.get(), + &pp->error); + } + + if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY && + !options.dynamic_sparsity) { + return ReorderProgramForSparseNormalCholesky( + options.sparse_linear_algebra_library_type, + *options.linear_solver_ordering, + pp->reduced_program.get(), + &pp->error); + } + + return true; +} + +// Configure and create a linear solver object. In doing so, if a +// sparse direct factorization based linear solver is being used, then +// find a fill reducing ordering and reorder the program as needed +// too. +bool SetupLinearSolver(PreprocessedProblem* pp) { + Solver::Options& options = pp->options; + if (options.linear_solver_ordering.get() == NULL) { + // If the user has not supplied a linear solver ordering, then we + // assume that they are giving all the freedom to us in choosing + // the best possible ordering. This intent can be indicated by + // putting all the parameter blocks in the same elimination group. + options.linear_solver_ordering.reset( + CreateDefaultLinearSolverOrdering(*pp->reduced_program)); + } else { + // If the user supplied an ordering, then check if the first + // elimination group is still non-empty after the reduced problem + // has been constructed. + // + // This is important for Schur type linear solvers, where the + // first elimination group is special -- it needs to be an + // independent set. + // + // If the first elimination group is empty, then we cannot use the + // user's requested linear solver (and a preconditioner as the + // case may be) so we must use a different one. + ParameterBlockOrdering* ordering = options.linear_solver_ordering.get(); + const int min_group_id = ordering->MinNonZeroGroup(); + ordering->Remove(pp->removed_parameter_blocks); + if (IsSchurType(options.linear_solver_type) && + min_group_id != ordering->MinNonZeroGroup()) { + AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver( + &options); + } + } + + // Reorder the program to reduce fill in and improve cache coherency + // of the Jacobian. + if (!ReorderProgram(pp)) { + return false; + } + + // Configure the linear solver. + pp->linear_solver_options = LinearSolver::Options(); + pp->linear_solver_options.min_num_iterations = + options.min_linear_solver_iterations; + pp->linear_solver_options.max_num_iterations = + options.max_linear_solver_iterations; + pp->linear_solver_options.type = options.linear_solver_type; + pp->linear_solver_options.preconditioner_type = options.preconditioner_type; + pp->linear_solver_options.visibility_clustering_type = + options.visibility_clustering_type; + pp->linear_solver_options.sparse_linear_algebra_library_type = + options.sparse_linear_algebra_library_type; + pp->linear_solver_options.dense_linear_algebra_library_type = + options.dense_linear_algebra_library_type; + pp->linear_solver_options.use_explicit_schur_complement = + options.use_explicit_schur_complement; + pp->linear_solver_options.dynamic_sparsity = options.dynamic_sparsity; + pp->linear_solver_options.num_threads = options.num_linear_solver_threads; + + // 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. + pp->linear_solver_options.use_postordering = options.use_postordering; + if (options.linear_solver_type == SPARSE_SCHUR && + options.sparse_linear_algebra_library_type == SUITE_SPARSE && + !SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) { + pp->linear_solver_options.use_postordering = true; + } + + OrderingToGroupSizes(options.linear_solver_ordering.get(), + &pp->linear_solver_options.elimination_groups); + + // Schur type solvers expect at least two elimination groups. If + // there is only one elimination group, then it is guaranteed that + // this group only contains e_blocks. Thus we add a dummy + // elimination group with zero blocks in it. + if (IsSchurType(pp->linear_solver_options.type) && + pp->linear_solver_options.elimination_groups.size() == 1) { + pp->linear_solver_options.elimination_groups.push_back(0); + } + + pp->linear_solver.reset(LinearSolver::Create(pp->linear_solver_options)); + return (pp->linear_solver.get() != NULL); +} + +// Configure and create the evaluator. +bool SetupEvaluator(PreprocessedProblem* pp) { + const Solver::Options& options = pp->options; + pp->evaluator_options = Evaluator::Options(); + pp->evaluator_options.linear_solver_type = options.linear_solver_type; + pp->evaluator_options.num_eliminate_blocks = 0; + if (IsSchurType(options.linear_solver_type)) { + pp->evaluator_options.num_eliminate_blocks = + options + .linear_solver_ordering + ->group_to_elements().begin() + ->second.size(); + } + + pp->evaluator_options.num_threads = options.num_threads; + pp->evaluator_options.dynamic_sparsity = options.dynamic_sparsity; + pp->evaluator.reset(Evaluator::Create(pp->evaluator_options, + pp->reduced_program.get(), + &pp->error)); + + return (pp->evaluator.get() != NULL); +} + +// If the user requested inner iterations, then find an inner +// iteration ordering as needed and configure and create a +// CoordinateDescentMinimizer object to perform the inner iterations. +bool SetupInnerIterationMinimizer(PreprocessedProblem* pp) { + Solver::Options& options = pp->options; + if (!options.use_inner_iterations) { + return true; + } + + // With just one parameter block, the outer iteration of the trust + // region method and inner iterations are doing exactly the same + // thing, and thus inner iterations are not needed. + if (pp->reduced_program->NumParameterBlocks() == 1) { + LOG(WARNING) << "Reduced problem only contains one parameter block." + << "Disabling inner iterations."; + return true; + } + + if (options.inner_iteration_ordering.get() != NULL) { + // If the user supplied an ordering, then remove the set of + // inactive parameter blocks from it + options.inner_iteration_ordering->Remove(pp->removed_parameter_blocks); + if (options.inner_iteration_ordering->NumElements() == 0) { + LOG(WARNING) << "No remaining elements in the inner iteration ordering."; + return true; + } + + // Validate the reduced ordering. + if (!CoordinateDescentMinimizer::IsOrderingValid( + *pp->reduced_program, + *options.inner_iteration_ordering, + &pp->error)) { + return false; + } + } else { + // The user did not supply an ordering, so create one. + options.inner_iteration_ordering.reset( + CoordinateDescentMinimizer::CreateOrdering(*pp->reduced_program)); + } + + pp->inner_iteration_minimizer.reset(new CoordinateDescentMinimizer); + return pp->inner_iteration_minimizer->Init(*pp->reduced_program, + pp->problem->parameter_map(), + *options.inner_iteration_ordering, + &pp->error); +} + +// Configure and create a TrustRegionMinimizer object. +void SetupMinimizerOptions(PreprocessedProblem* pp) { + const Solver::Options& options = pp->options; + + SetupCommonMinimizerOptions(pp); + pp->minimizer_options.is_constrained = + pp->reduced_program->IsBoundsConstrained(); + pp->minimizer_options.jacobian.reset(pp->evaluator->CreateJacobian()); + pp->minimizer_options.inner_iteration_minimizer = + pp->inner_iteration_minimizer; + + TrustRegionStrategy::Options strategy_options; + strategy_options.linear_solver = pp->linear_solver.get(); + strategy_options.initial_radius = + options.initial_trust_region_radius; + strategy_options.max_radius = options.max_trust_region_radius; + strategy_options.min_lm_diagonal = options.min_lm_diagonal; + strategy_options.max_lm_diagonal = options.max_lm_diagonal; + strategy_options.trust_region_strategy_type = + options.trust_region_strategy_type; + strategy_options.dogleg_type = options.dogleg_type; + pp->minimizer_options.trust_region_strategy.reset( + CHECK_NOTNULL(TrustRegionStrategy::Create(strategy_options))); +} + +} // namespace + +TrustRegionPreprocessor::~TrustRegionPreprocessor() { +} + +bool TrustRegionPreprocessor::Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* pp) { + CHECK_NOTNULL(pp); + pp->options = options; + ChangeNumThreadsIfNeeded(&pp->options); + + pp->problem = problem; + Program* program = problem->mutable_program(); + if (!IsProgramValid(*program, &pp->error)) { + return false; + } + + pp->reduced_program.reset( + program->CreateReducedProgram(&pp->removed_parameter_blocks, + &pp->fixed_cost, + &pp->error)); + + if (pp->reduced_program.get() == NULL) { + return false; + } + + if (pp->reduced_program->NumParameterBlocks() == 0) { + // The reduced problem has no parameter or residual blocks. There + // is nothing more to do. + return true; + } + + if (!SetupLinearSolver(pp) || + !SetupEvaluator(pp) || + !SetupInnerIterationMinimizer(pp)) { + return false; + } + + SetupMinimizerOptions(pp); + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/trust_region_preprocessor.h b/extern/ceres/internal/ceres/trust_region_preprocessor.h new file mode 100644 index 00000000000..a6631ab3d40 --- /dev/null +++ b/extern/ceres/internal/ceres/trust_region_preprocessor.h @@ -0,0 +1,50 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: sameragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ +#define CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ + +#include "ceres/preprocessor.h" + +namespace ceres { +namespace internal { + +class TrustRegionPreprocessor : public Preprocessor { + public: + virtual ~TrustRegionPreprocessor(); + virtual bool Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* preprocessed_problem); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ diff --git a/extern/ceres/internal/ceres/trust_region_strategy.cc b/extern/ceres/internal/ceres/trust_region_strategy.cc new file mode 100644 index 00000000000..2db6a6c899b --- /dev/null +++ b/extern/ceres/internal/ceres/trust_region_strategy.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#include "ceres/trust_region_strategy.h" +#include "ceres/dogleg_strategy.h" +#include "ceres/levenberg_marquardt_strategy.h" + +namespace ceres { +namespace internal { + +TrustRegionStrategy::~TrustRegionStrategy() {} + +TrustRegionStrategy* TrustRegionStrategy::Create(const Options& options) { + switch (options.trust_region_strategy_type) { + case LEVENBERG_MARQUARDT: + return new LevenbergMarquardtStrategy(options); + case DOGLEG: + return new DoglegStrategy(options); + default: + LOG(FATAL) << "Unknown trust region strategy: " + << options.trust_region_strategy_type; + } + + LOG(FATAL) << "Unknown trust region strategy: " + << options.trust_region_strategy_type; + return NULL; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/trust_region_strategy.h b/extern/ceres/internal/ceres/trust_region_strategy.h new file mode 100644 index 00000000000..9560e67459a --- /dev/null +++ b/extern/ceres/internal/ceres/trust_region_strategy.h @@ -0,0 +1,164 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ +#define CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ + +#include <string> +#include "ceres/internal/port.h" +#include "ceres/linear_solver.h" + +namespace ceres { +namespace internal { + +class LinearSolver; +class SparseMatrix; + +// Interface for classes implementing various trust region strategies +// for nonlinear least squares problems. +// +// The object is expected to maintain and update a trust region +// radius, which it then uses to solve for the trust region step using +// the jacobian matrix and residual vector. +// +// Here the term trust region radius is used loosely, as the strategy +// is free to treat it as guidance and violate it as need be. e.g., +// the LevenbergMarquardtStrategy uses the inverse of the trust region +// radius to scale the damping term, which controls the step size, but +// does not set a hard limit on its size. +class TrustRegionStrategy { + public: + struct Options { + Options() + : trust_region_strategy_type(LEVENBERG_MARQUARDT), + initial_radius(1e4), + max_radius(1e32), + min_lm_diagonal(1e-6), + max_lm_diagonal(1e32), + dogleg_type(TRADITIONAL_DOGLEG) { + } + + TrustRegionStrategyType trust_region_strategy_type; + // Linear solver used for actually solving the trust region step. + LinearSolver* linear_solver; + double initial_radius; + double max_radius; + + // Minimum and maximum values of the diagonal damping matrix used + // by LevenbergMarquardtStrategy. The DoglegStrategy also uses + // these bounds to construct a regularizing diagonal to ensure + // that the Gauss-Newton step computation is of full rank. + double min_lm_diagonal; + double max_lm_diagonal; + + // Further specify which dogleg method to use + DoglegType dogleg_type; + }; + + // 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. + std::string dump_filename_base; + DumpFormatType dump_format_type; + }; + + struct Summary { + Summary() + : residual_norm(0.0), + num_iterations(-1), + termination_type(LINEAR_SOLVER_FAILURE) { + } + + // If the trust region problem is, + // + // 1/2 x'Ax + b'x + c, + // + // then + // + // residual_norm = |Ax -b| + double residual_norm; + + // Number of iterations used by the linear solver. If a linear + // solver was not called (e.g., DogLegStrategy after an + // unsuccessful step), then this would be zero. + int num_iterations; + + // Status of the linear solver used to solve the Newton system. + LinearSolverTerminationType termination_type; + }; + + virtual ~TrustRegionStrategy(); + + // Use the current radius to solve for the trust region step. + virtual Summary ComputeStep(const PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals, + double* step) = 0; + + // Inform the strategy that the current step has been accepted, and + // that the ratio of the decrease in the non-linear objective to the + // decrease in the trust region model is step_quality. + virtual void StepAccepted(double step_quality) = 0; + + // Inform the strategy that the current step has been rejected, and + // that the ratio of the decrease in the non-linear objective to the + // decrease in the trust region model is step_quality. + virtual void StepRejected(double step_quality) = 0; + + // Inform the strategy that the current step has been rejected + // because it was found to be numerically invalid. + // StepRejected/StepAccepted will not be called for this step, and + // the strategy is free to do what it wants with this information. + virtual void StepIsInvalid() = 0; + + // Current trust region radius. + virtual double Radius() const = 0; + + // Factory. + static TrustRegionStrategy* Create(const Options& options); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ diff --git a/extern/ceres/internal/ceres/types.cc b/extern/ceres/internal/ceres/types.cc new file mode 100644 index 00000000000..f86fb78eb8c --- /dev/null +++ b/extern/ceres/internal/ceres/types.cc @@ -0,0 +1,395 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <algorithm> +#include <cctype> +#include <string> +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +using std::string; + +#define CASESTR(x) case x: return #x +#define STRENUM(x) if (value == #x) { *type = x; return true;} + +static void UpperCase(string* input) { + std::transform(input->begin(), input->end(), input->begin(), ::toupper); +} + +const char* LinearSolverTypeToString(LinearSolverType type) { + switch (type) { + CASESTR(DENSE_NORMAL_CHOLESKY); + CASESTR(DENSE_QR); + CASESTR(SPARSE_NORMAL_CHOLESKY); + CASESTR(DENSE_SCHUR); + CASESTR(SPARSE_SCHUR); + CASESTR(ITERATIVE_SCHUR); + CASESTR(CGNR); + default: + return "UNKNOWN"; + } +} + +bool StringToLinearSolverType(string value, LinearSolverType* type) { + UpperCase(&value); + STRENUM(DENSE_NORMAL_CHOLESKY); + STRENUM(DENSE_QR); + STRENUM(SPARSE_NORMAL_CHOLESKY); + STRENUM(DENSE_SCHUR); + STRENUM(SPARSE_SCHUR); + STRENUM(ITERATIVE_SCHUR); + STRENUM(CGNR); + return false; +} + +const char* PreconditionerTypeToString(PreconditionerType type) { + switch (type) { + CASESTR(IDENTITY); + CASESTR(JACOBI); + CASESTR(SCHUR_JACOBI); + CASESTR(CLUSTER_JACOBI); + CASESTR(CLUSTER_TRIDIAGONAL); + default: + return "UNKNOWN"; + } +} + +bool StringToPreconditionerType(string value, PreconditionerType* type) { + UpperCase(&value); + STRENUM(IDENTITY); + STRENUM(JACOBI); + STRENUM(SCHUR_JACOBI); + STRENUM(CLUSTER_JACOBI); + STRENUM(CLUSTER_TRIDIAGONAL); + return false; +} + +const char* SparseLinearAlgebraLibraryTypeToString( + SparseLinearAlgebraLibraryType type) { + switch (type) { + CASESTR(SUITE_SPARSE); + CASESTR(CX_SPARSE); + CASESTR(EIGEN_SPARSE); + CASESTR(NO_SPARSE); + default: + return "UNKNOWN"; + } +} + +bool StringToSparseLinearAlgebraLibraryType( + string value, + SparseLinearAlgebraLibraryType* type) { + UpperCase(&value); + STRENUM(SUITE_SPARSE); + STRENUM(CX_SPARSE); + STRENUM(EIGEN_SPARSE); + STRENUM(NO_SPARSE); + 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); + CASESTR(DOGLEG); + default: + return "UNKNOWN"; + } +} + +bool StringToTrustRegionStrategyType(string value, + TrustRegionStrategyType* type) { + UpperCase(&value); + STRENUM(LEVENBERG_MARQUARDT); + STRENUM(DOGLEG); + return false; +} + +const char* DoglegTypeToString(DoglegType type) { + switch (type) { + CASESTR(TRADITIONAL_DOGLEG); + CASESTR(SUBSPACE_DOGLEG); + default: + return "UNKNOWN"; + } +} + +bool StringToDoglegType(string value, DoglegType* type) { + UpperCase(&value); + STRENUM(TRADITIONAL_DOGLEG); + STRENUM(SUBSPACE_DOGLEG); + return false; +} + +const char* MinimizerTypeToString(MinimizerType type) { + switch (type) { + CASESTR(TRUST_REGION); + CASESTR(LINE_SEARCH); + default: + return "UNKNOWN"; + } +} + +bool StringToMinimizerType(string value, MinimizerType* type) { + UpperCase(&value); + STRENUM(TRUST_REGION); + STRENUM(LINE_SEARCH); + return false; +} + +const char* LineSearchDirectionTypeToString(LineSearchDirectionType type) { + switch (type) { + CASESTR(STEEPEST_DESCENT); + CASESTR(NONLINEAR_CONJUGATE_GRADIENT); + CASESTR(LBFGS); + CASESTR(BFGS); + default: + return "UNKNOWN"; + } +} + +bool StringToLineSearchDirectionType(string value, + LineSearchDirectionType* type) { + UpperCase(&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"; + } +} + +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; +} + +const char* NonlinearConjugateGradientTypeToString( + NonlinearConjugateGradientType type) { + switch (type) { + CASESTR(FLETCHER_REEVES); + CASESTR(POLAK_RIBIERE); + CASESTR(HESTENES_STIEFEL); + default: + return "UNKNOWN"; + } +} + +bool StringToNonlinearConjugateGradientType( + string value, + NonlinearConjugateGradientType* type) { + UpperCase(&value); + STRENUM(FLETCHER_REEVES); + STRENUM(POLAK_RIBIERE); + STRENUM(HESTENES_STIEFEL); + return false; +} + +const char* CovarianceAlgorithmTypeToString( + CovarianceAlgorithmType type) { + switch (type) { + CASESTR(DENSE_SVD); + CASESTR(EIGEN_SPARSE_QR); + CASESTR(SUITE_SPARSE_QR); + default: + return "UNKNOWN"; + } +} + +bool StringToCovarianceAlgorithmType( + string value, + CovarianceAlgorithmType* type) { + UpperCase(&value); + STRENUM(DENSE_SVD); + STRENUM(EIGEN_SPARSE_QR); + STRENUM(SUITE_SPARSE_QR); + return false; +} + +const char* NumericDiffMethodTypeToString( + NumericDiffMethodType type) { + switch (type) { + CASESTR(CENTRAL); + CASESTR(FORWARD); + CASESTR(RIDDERS); + default: + return "UNKNOWN"; + } +} + +bool StringToNumericDiffMethodType( + string value, + NumericDiffMethodType* type) { + UpperCase(&value); + STRENUM(CENTRAL); + STRENUM(FORWARD); + STRENUM(RIDDERS); + return false; +} + +const char* VisibilityClusteringTypeToString( + VisibilityClusteringType type) { + switch (type) { + CASESTR(CANONICAL_VIEWS); + CASESTR(SINGLE_LINKAGE); + default: + return "UNKNOWN"; + } +} + +bool StringToVisibilityClusteringType( + string value, + VisibilityClusteringType* type) { + UpperCase(&value); + STRENUM(CANONICAL_VIEWS); + STRENUM(SINGLE_LINKAGE); + return false; +} + +const char* TerminationTypeToString(TerminationType type) { + switch (type) { + CASESTR(CONVERGENCE); + CASESTR(NO_CONVERGENCE); + CASESTR(FAILURE); + CASESTR(USER_SUCCESS); + CASESTR(USER_FAILURE); + default: + return "UNKNOWN"; + } +} + +#undef CASESTR +#undef STRENUM + +bool IsSchurType(LinearSolverType type) { + return ((type == SPARSE_SCHUR) || + (type == DENSE_SCHUR) || + (type == ITERATIVE_SCHUR)); +} + +bool IsSparseLinearAlgebraLibraryTypeAvailable( + SparseLinearAlgebraLibraryType type) { + if (type == SUITE_SPARSE) { +#ifdef CERES_NO_SUITESPARSE + return false; +#else + return true; +#endif + } + + if (type == CX_SPARSE) { +#ifdef CERES_NO_CXSPARSE + return false; +#else + return true; +#endif + } + + if (type == EIGEN_SPARSE) { +#ifdef CERES_USE_EIGEN_SPARSE + return true; +#else + return false; +#endif + } + + LOG(WARNING) << "Unknown sparse linear algebra library " << type; + 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/ceres/internal/ceres/visibility_based_preconditioner.h b/extern/ceres/internal/ceres/visibility_based_preconditioner.h new file mode 100644 index 00000000000..a627c13523c --- /dev/null +++ b/extern/ceres/internal/ceres/visibility_based_preconditioner.h @@ -0,0 +1,233 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Preconditioners for linear systems that arise in Structure from +// Motion problems. VisibilityBasedPreconditioner implements: +// +// CLUSTER_JACOBI +// CLUSTER_TRIDIAGONAL +// +// Detailed descriptions of these preconditions beyond what is +// documented here can be found in +// +// Visibility Based Preconditioning for Bundle Adjustment +// A. Kushal & S. Agarwal, CVPR 2012. +// +// http://www.cs.washington.edu/homes/sagarwal/vbp.pdf +// +// The two preconditioners share enough code that its most efficient +// to implement them as part of the same code base. + +#ifndef CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ +#define CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/collections_port.h" +#include "ceres/graph.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_solver.h" +#include "ceres/preconditioner.h" +#include "ceres/suitesparse.h" + +namespace ceres { +namespace internal { + +class BlockRandomAccessSparseMatrix; +class BlockSparseMatrix; +struct CompressedRowBlockStructure; +class SchurEliminatorBase; + +// This class implements visibility based preconditioners for +// Structure from Motion/Bundle Adjustment problems. The name +// VisibilityBasedPreconditioner comes from the fact that the sparsity +// structure of the preconditioner matrix is determined by analyzing +// the visibility structure of the scene, i.e. which cameras see which +// points. +// +// The key idea of visibility based preconditioning is to identify +// cameras that we expect have strong interactions, and then using the +// entries in the Schur complement matrix corresponding to these +// camera pairs as an approximation to the full Schur complement. +// +// CLUSTER_JACOBI identifies these camera pairs by clustering cameras, +// and considering all non-zero camera pairs within each cluster. The +// clustering in the current implementation is done using the +// Canonical Views algorithm of Simon et al. (see +// canonical_views_clustering.h). For the purposes of clustering, the +// similarity or the degree of interaction between a pair of cameras +// is measured by counting the number of points visible in both the +// cameras. Thus the name VisibilityBasedPreconditioner. Further, if we +// were to permute the parameter blocks such that all the cameras in +// the same cluster occur contiguously, the preconditioner matrix will +// be a block diagonal matrix with blocks corresponding to the +// clusters. Thus in analogy with the Jacobi preconditioner we refer +// to this as the CLUSTER_JACOBI preconditioner. +// +// CLUSTER_TRIDIAGONAL adds more mass to the CLUSTER_JACOBI +// preconditioner by considering the interaction between clusters and +// identifying strong interactions between cluster pairs. This is done +// by constructing a weighted graph on the clusters, with the weight +// on the edges connecting two clusters proportional to the number of +// 3D points visible to cameras in both the clusters. A degree-2 +// maximum spanning forest is identified in this graph and the camera +// pairs contained in the edges of this forest are added to the +// preconditioner. The detailed reasoning for this construction is +// explained in the paper mentioned above. +// +// Degree-2 spanning trees and forests have the property that they +// correspond to tri-diagonal matrices. Thus there exist a permutation +// of the camera blocks under which the CLUSTER_TRIDIAGONAL +// preconditioner matrix is a block tridiagonal matrix, and thus the +// name for the preconditioner. +// +// Thread Safety: This class is NOT thread safe. +// +// Example usage: +// +// LinearSolver::Options options; +// options.preconditioner_type = CLUSTER_JACOBI; +// options.elimination_groups.push_back(num_points); +// options.elimination_groups.push_back(num_cameras); +// VisibilityBasedPreconditioner preconditioner( +// *A.block_structure(), options); +// preconditioner.Update(A, NULL); +// preconditioner.RightMultiply(x, y); +// +#ifndef CERES_NO_SUITESPARSE +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 + // to determine the sparsity structure of the preconditioner matrix. + // + // It has the same structural requirement as other Schur complement + // based solvers. Please see schur_eliminator.h for more details. + VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, + const Preconditioner::Options& options); + virtual ~VisibilityBasedPreconditioner(); + + // Preconditioner interface + 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); + void InitEliminator(const CompressedRowBlockStructure& bs); + LinearSolverTerminationType Factorize(); + void ScaleOffDiagonalCells(); + + void ClusterCameras(const std::vector<std::set<int> >& visibility); + void FlattenMembershipMap(const HashMap<int, int>& membership_map, + std::vector<int>* membership_vector) const; + void ComputeClusterVisibility( + const std::vector<std::set<int> >& visibility, + std::vector<std::set<int> >* cluster_visibility) const; + WeightedGraph<int>* CreateClusterGraph( + const std::vector<std::set<int> >& visibility) const; + void ForestToClusterPairs(const WeightedGraph<int>& forest, + HashSet<std::pair<int, int> >* cluster_pairs) const; + void ComputeBlockPairsInPreconditioner(const CompressedRowBlockStructure& bs); + bool IsBlockPairInPreconditioner(int block1, int block2) const; + bool IsBlockPairOffDiagonal(int block1, int block2) const; + + Preconditioner::Options options_; + + // Number of parameter blocks in the schur complement. + int num_blocks_; + int num_clusters_; + + // Sizes of the blocks in the schur complement. + std::vector<int> block_size_; + + // Mapping from cameras to clusters. + std::vector<int> cluster_membership_; + + // Non-zero camera pairs from the schur complement matrix that are + // present in the preconditioner, sorted by row (first element of + // each pair), then column (second). + std::set<std::pair<int, int> > block_pairs_; + + // Set of cluster pairs (including self pairs (i,i)) in the + // preconditioner. + HashSet<std::pair<int, int> > cluster_pairs_; + scoped_ptr<SchurEliminatorBase> eliminator_; + + // Preconditioner matrix. + scoped_ptr<BlockRandomAccessSparseMatrix> m_; + + // RightMultiply is a const method for LinearOperators. It is + // implemented using CHOLMOD's sparse triangular matrix solve + // function. This however requires non-const access to the + // SuiteSparse context object, even though it does not result in any + // of the state of the preconditioner being modified. + SuiteSparse ss_; + + // Symbolic and numeric factorization of the preconditioner. + cholmod_factor* factor_; + + // Temporary vector used by RightMultiply. + cholmod_dense* tmp_rhs_; + CERES_DISALLOW_COPY_AND_ASSIGN(VisibilityBasedPreconditioner); +}; +#else // SuiteSparse +// If SuiteSparse is not compiled in, the preconditioner is not +// available. +class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { + public: + VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, + const Preconditioner::Options& options) { + LOG(FATAL) << "Visibility based preconditioning is not available. Please " + "build Ceres with SuiteSparse."; + } + virtual ~VisibilityBasedPreconditioner() {} + virtual void RightMultiply(const double* x, double* y) const {} + virtual void LeftMultiply(const double* x, double* y) const {} + virtual int num_rows() const { return -1; } + virtual int num_cols() const { return -1; } + + private: + bool UpdateImpl(const BlockSparseMatrix& A, const double* D) { + return false; + } +}; +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ diff --git a/extern/ceres/internal/ceres/wall_time.cc b/extern/ceres/internal/ceres/wall_time.cc new file mode 100644 index 00000000000..c353973cc3e --- /dev/null +++ b/extern/ceres/internal/ceres/wall_time.cc @@ -0,0 +1,96 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: strandmark@google.com (Petter Strandmark) + +#include "ceres/wall_time.h" + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#else +#include <ctime> +#endif + +#ifdef _WIN32 +#include <windows.h> +#else +#include <sys/time.h> +#endif + +namespace ceres { +namespace internal { + +double WallTimeInSeconds() { +#ifdef CERES_USE_OPENMP + return omp_get_wtime(); +#else +#ifdef _WIN32 + return static_cast<double>(std::time(NULL)); +#else + timeval time_val; + gettimeofday(&time_val, NULL); + return (time_val.tv_sec + time_val.tv_usec * 1e-6); +#endif +#endif +} + +EventLogger::EventLogger(const std::string& logger_name) + : start_time_(WallTimeInSeconds()), + last_event_time_(start_time_), + events_("") { + StringAppendF(&events_, + "\n%s\n Delta Cumulative\n", + logger_name.c_str()); +} + +EventLogger::~EventLogger() { + if (VLOG_IS_ON(3)) { + AddEvent("Total"); + VLOG(2) << "\n" << events_ << "\n"; + } +} + +void EventLogger::AddEvent(const std::string& event_name) { + if (!VLOG_IS_ON(3)) { + return; + } + + const double current_time = WallTimeInSeconds(); + const double relative_time_delta = current_time - last_event_time_; + const double absolute_time_delta = current_time - start_time_; + last_event_time_ = current_time; + + StringAppendF(&events_, + " %30s : %10.5f %10.5f\n", + event_name.c_str(), + relative_time_delta, + absolute_time_delta); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/wall_time.h b/extern/ceres/internal/ceres/wall_time.h new file mode 100644 index 00000000000..966aa67cab6 --- /dev/null +++ b/extern/ceres/internal/ceres/wall_time.h @@ -0,0 +1,88 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: strandmark@google.com (Petter Strandmark) + +#ifndef CERES_INTERNAL_WALL_TIME_H_ +#define CERES_INTERNAL_WALL_TIME_H_ + +#include <map> +#include <string> +#include "ceres/internal/port.h" +#include "ceres/stringprintf.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Returns time, in seconds, from some arbitrary starting point. If +// OpenMP is available then the high precision openmp_get_wtime() +// function is used. Otherwise on unixes, gettimeofday is used. The +// granularity is in seconds on windows systems. +double WallTimeInSeconds(); + +// Log a series of events, recording for each event the time elapsed +// since the last event and since the creation of the object. +// +// The information is output to VLOG(3) upon destruction. A +// name::Total event is added as the final event right before +// destruction. +// +// Example usage: +// +// void Foo() { +// EventLogger event_logger("Foo"); +// Bar1(); +// event_logger.AddEvent("Bar1") +// Bar2(); +// event_logger.AddEvent("Bar2") +// Bar3(); +// } +// +// Will produce output that looks like +// +// Foo +// Bar1: time1 time1 +// Bar2: time2 time1 + time2; +// Total: time3 time1 + time2 + time3; +class EventLogger { + public: + explicit EventLogger(const std::string& logger_name); + ~EventLogger(); + void AddEvent(const std::string& event_name); + + private: + const double start_time_; + double last_event_time_; + std::string events_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_WALL_TIME_H_ diff --git a/extern/ceres/mkfiles.sh b/extern/ceres/mkfiles.sh new file mode 100755 index 00000000000..cb07663e94a --- /dev/null +++ b/extern/ceres/mkfiles.sh @@ -0,0 +1,5 @@ +#!/bin/sh + +find ./include/ -type f | sed -r 's/^\.\///' | sort > files.txt +find ./internal/ -type f | sed -r 's/^\.\///' | sort >> files.txt +find ./config/ -type f | sed -r 's/^\.\///' | sort >> files.txt diff --git a/extern/ceres/patches/series b/extern/ceres/patches/series new file mode 100644 index 00000000000..e69de29bb2d --- /dev/null +++ b/extern/ceres/patches/series |