diff options
author | Sergey Sharybin <sergey.vfx@gmail.com> | 2016-11-01 13:29:33 +0300 |
---|---|---|
committer | Sergey Sharybin <sergey.vfx@gmail.com> | 2016-11-01 13:29:33 +0300 |
commit | bf1e9bc613377a4a4d5dcf9f50e757a4feb0928f (patch) | |
tree | 9c7daacbf8a72154cb0fce19acade5ff3990eaca /extern | |
parent | cf8f6d1dbcfc86328d5917298e81070a826aea7d (diff) |
Ceres: Update to the latest actual version
Brings all the fixes and improvements done in upstream within the last 13 months.
Diffstat (limited to 'extern')
45 files changed, 2709 insertions, 1632 deletions
diff --git a/extern/ceres/CMakeLists.txt b/extern/ceres/CMakeLists.txt index 2ad8c543088..a6e9cd9c356 100644 --- a/extern/ceres/CMakeLists.txt +++ b/extern/ceres/CMakeLists.txt @@ -73,10 +73,12 @@ set(SRC 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_checker.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/is_close.cc internal/ceres/iterative_schur_complement_solver.cc internal/ceres/lapack.cc internal/ceres/levenberg_marquardt_strategy.cc @@ -116,6 +118,7 @@ set(SRC internal/ceres/triplet_sparse_matrix.cc internal/ceres/trust_region_minimizer.cc internal/ceres/trust_region_preprocessor.cc + internal/ceres/trust_region_step_evaluator.cc internal/ceres/trust_region_strategy.cc internal/ceres/types.cc internal/ceres/wall_time.cc @@ -204,6 +207,7 @@ set(SRC internal/ceres/householder_vector.h internal/ceres/implicit_schur_complement.h internal/ceres/integral_types.h + internal/ceres/is_close.h internal/ceres/iterative_schur_complement_solver.h internal/ceres/lapack.h internal/ceres/levenberg_marquardt_strategy.h @@ -248,6 +252,7 @@ set(SRC internal/ceres/triplet_sparse_matrix.h internal/ceres/trust_region_minimizer.h internal/ceres/trust_region_preprocessor.h + internal/ceres/trust_region_step_evaluator.h internal/ceres/trust_region_strategy.h internal/ceres/visibility_based_preconditioner.h internal/ceres/wall_time.h diff --git a/extern/ceres/ChangeLog b/extern/ceres/ChangeLog index 0e6c195174c..ae8d42a7c95 100644 --- a/extern/ceres/ChangeLog +++ b/extern/ceres/ChangeLog @@ -1,659 +1,588 @@ -commit aef9c9563b08d5f39eee1576af133a84749d1b48 -Author: Alessandro Gentilini <agentilini@gmail.com> -Date: Tue Oct 6 20:43:45 2015 +0200 +commit 8590e6e8e057adba4ec0083446d00268565bb444 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu Oct 27 12:29:37 2016 -0700 - Add test for Bessel functions. + Remove two checks from rotation.h + + This allows rotation.h to remove its dependency on glog. - Change-Id: Ief5881e8027643d7ef627e60a88fdbad17f3d884 + Change-Id: Ia6aede93ee51a4bd4039570dc8edd100a7045329 -commit 49c86018e00f196c4aa9bd25daccb9919917efee -Author: Alessandro Gentilini <agentilini@gmail.com> -Date: Wed Sep 23 21:59:44 2015 +0200 +commit e892499e8d8977b9178a760348bdd201ec5f3489 +Author: Je Hyeong Hong <jhh37@outlook.com> +Date: Tue Oct 18 22:49:11 2016 +0100 - Add Bessel functions in order to use them in residual code. + Relax the tolerance in QuaternionParameterizationTestHelper. - See "How can I use the Bessel function in the residual function?" at - https://groups.google.com/d/msg/ceres-solver/Vh1gpqac8v0/NIK1EiWJCAAJ + This commit relaxes the tolerance value for comparing between the actual + local matrix and the expected local matrix. Without this fix, + EigenQuaternionParameterization.ZeroTest could fail as the difference + exactly matches the value of std::numeric_limits<double>::epsilon(). - Change-Id: I3e80d9f9d1cadaf7177076e493ff46ace5233b76 + Change-Id: Ic4d3f26c0acdf5f16fead80dfdc53df9e7dabbf9 -commit dfb201220c034fde00a242d0533bef3f73b2907d -Author: Simon Rutishauser <simon.rutishauser@pix4d.com> -Date: Tue Oct 13 07:33:58 2015 +0200 +commit 7ed9e2fb7f1dff264c5e4fbaa89ee1c4c99df269 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed Oct 19 04:45:23 2016 -0700 - Make miniglog threadsafe on non-windows system by using - localtime_r() instead of localtime() for time formatting + Occured -> Occurred. - Change-Id: Ib8006c685cd8ed4f374893bef56c4061ca2c9747 + Thanks to Phillip Huebner for reporting this. + + Change-Id: I9cddfbb373aeb496961d08e434fe661bff4abd29 -commit 41455566ac633e55f222bce7c4d2cb4cc33d5c72 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Mon Sep 28 22:43:42 2015 +0100 +commit b82f97279682962d8c8ae1b6d9e801ba072a0ab1 +Author: Je Hyeong Hong <jhh37@outlook.com> +Date: Tue Oct 18 21:18:32 2016 +0100 - Remove link-time optimisation (LTO). + Fix a test error in autodiff_test.cc. - - 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. + Previously, the test for the projective camera model would fail as no + tolerance is set in line 144. To resolve this, this commit changes + assert_equal to assert_near. - Change-Id: I17b52957faefbdeff0aa40846dc9b342db1b02e3 + Change-Id: I6cd3379083b1a10c7cd0a9cc83fd6962bb993cc9 -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 +commit 5690b447de5beed6bdda99b7f30f372283c2fb1a Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sat Sep 26 14:10:15 2015 -0700 +Date: Thu Oct 13 09:52:02 2016 -0700 - Documentation updates. + Fix documentation source for templated functions in rotation.h - 1. Fix a typo in the Trust Region algorithm. - 2. Add ARL in the list of users. - 3. Update the version history. + Change-Id: Ic1b2e6f0e6eb9914f419fd0bb5af77b66252e57c + +commit 2f8f98f7e8940e465de126fb51282396f42bea20 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu Oct 13 09:35:18 2016 -0700 + + Prepare for 1.12.0RC1 - Change-Id: Ic286e8ef1a71af07f3890b7592dd3aed9c5f87ce + Change-Id: I23eaf0b46117a01440143001b74dacfa5e57cbf0 -commit 90e32a8dc437dfb0e6747ce15a1f3193c13b7d5b -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Mon Sep 21 21:08:25 2015 +0100 +commit 55c12d2e9569fe4aeac3ba688ac36810935a37ba +Author: Damon Kohler <damonkohler@google.com> +Date: Wed Oct 5 16:30:31 2016 +0200 + + Adds package.xml to support Catkin. + + Change-Id: I8ad4d36a8b036417604a54644e0bb70dd1615feb + +commit 0bcce6565202f5476e40f12afc0a99eb44bd9dfb +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Mon Oct 10 23:30:42 2016 -0700 - Use old minimum iOS version flags on Xcode < 7.0. + Fix tabs in Android.mk - - The newer style, which are more specific and match the SDK names - are not available on Xcode < 7.0. + Change-Id: Ie5ab9a8ba2b727721565e1ded242609b6df5f8f5 + +commit e6ffe2667170d2fc435443685c0163396fc52d7b +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Mon Oct 10 22:47:08 2016 -0700 + + Update the version history. - Change-Id: I2f07a0365183d2781157cdb05fd49b30ae001ac5 + Change-Id: I9a57b0541d6cebcb695ecb364a1d4ca04ea4e06c -commit 26cd5326a1fb99ae02c667eab9942e1308046984 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Mon Sep 21 10:16:01 2015 +0100 +commit 0a4ccb7ee939ab35b22e26758401e039b033b176 +Author: David Gossow <dgossow@google.com> +Date: Wed Sep 7 21:38:12 2016 +0200 - Add gtest-specific flags when building/using as a shared library. + Relaxing Jacobian matching in Gradient Checker test. - - Currently these flags are only used to define the relevant DLL export - prefix for Windows. + Any result of an arithmetic operation on floating-point matrices + should never be checked for strict equality with some expected + value, due to limited floating point precision on different machines. + This fixes some occurences of exact checks in the gradient checker + unit test that were causing problems on some platforms. - Change-Id: I0c05207b512cb4a985390aefc779b91febdabb38 + Change-Id: I48e804c9c705dc485ce74ddfe51037d4957c8fcb -commit c4c79472112a49bc1340da0074af2d15b1c89749 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sun Sep 20 18:26:59 2015 +0100 +commit ee44fc91b59584921c1d1c8db153fda6d633b092 +Author: Je Hyeong Hong <jhh37@outlook.com> +Date: Mon Oct 3 12:19:30 2016 +0100 - Clean up iOS.cmake to use xcrun/xcodebuild & libtool. + Fix an Intel compiler error in covariance_impl.cc. - - 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. + Intel C compiler strictly asks for parallel loops with collapse to be + perfectly nested. Otherwise, compiling Ceres with ICC will throw an + error at line 348 of covariance_impl.cc. - Change-Id: I5f83a60607db34d461ebe85f9dce861f53d98277 + Change-Id: I1ecb68e89b7faf79e4153dfe6675c390d1780db4 -commit 155765bbb358f1d19f072a4b54825faf1c059910 +commit 9026d69d1ce1e0bcd21debd54a38246d85c7c6e4 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Sep 16 06:56:08 2015 -0700 +Date: Thu Sep 22 17:20:14 2016 -0700 + + Allow SubsetParameterization to hold all parameters constant + + 1. SubsetParameterization can now be constructed such that all + parameters are constant. This is required for it be used as part + of a ProductParameterization to hold a part of parameter block + constant. For example, a parameter block consisting of a rotation + as a quaternion and a translation vector can now have a local + parameterization where the translation part is constant and the + quaternion part has a QuaternionParameterization associated with it. + + 2. The check for the tangent space of a parameterization being + positive dimensional. We were not doing this check up till now + and the user could accidentally create parameterizations like this + and create a problem for themselves. This will ensure that even + though one can construct a SubsetParameterization where all + parameters are constant, you cannot actually use it as a local + parameterization for an entire parameter block. Which is how + it was before, but the check was inside the SubsetParameterization + constructor. + + 3. Added more tests and refactored existing tests to be more + granular. + + Change-Id: Ic0184a1f30e3bd8a416b02341781a9d98e855ff7 + +commit a36693f83da7a3fd19dce473d060231d4cc97499 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sat Sep 17 16:31:41 2016 -0700 - Import the latest version of gtest and gmock. + Update version history - Change-Id: I4b686c44bba823cab1dae40efa99e31340d2b52a + Change-Id: Ib2f0138ed7a1879ca3b2173e54092f7ae8dd5c9d -commit 0c4647b8f1496c97c6b9376d9c49ddc204aa08dd -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Wed Sep 16 20:01:11 2015 +0100 +commit 01e23e3d33178fdd050973666505c1080cfe04c3 +Author: David Gossow <dgossow@google.com> +Date: Thu Sep 8 12:22:28 2016 +0200 - Remove FAQ about increasing inlining threshold for Clang. + Removing duplicate include directive. - - 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 + Change-Id: I729ae6501497746d1bb615cb893ad592e16ddf3f -commit f4b768b69afcf282568f9ab3a3f0eb8078607468 +commit 99b8210cee92cb972267537fb44bebf56f812d52 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Sep 14 13:53:24 2015 -0700 +Date: Wed Sep 7 15:31:30 2016 -0700 - Lint changes from William Rucklidge + Update Android.mk to include new files. - Change-Id: I0dac2549a8fa2bfd12f745a8d8a0db623b7ec1ac + Change-Id: Id543ee7d2a65b65c868554a17f593c0a4958e873 -commit 5f2f05c726443e35767d677daba6d25dbc2d7ff8 +commit 195d8d13a6a3962ac39ef7fcdcc6add0216eb8bc Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Fri Sep 11 22:19:38 2015 -0700 +Date: Tue Sep 6 07:12:23 2016 -0700 - Refactor system_test + Remove two DCHECKs from CubicHermiteSpline. - 1. Move common test infrastructure into test_util. - 2. system_test now only contains powells function. - 3. Add bundle_adjustment_test. + They were present as debugging checks but were causing problems + with the build on 32bit i386 due to numerical cancellation issues, + where x ~ -epsilon. - 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. + Removing these checks only changes the behaviour in Debug mode. + We are already handling such small negative numbers in production + if they occur. All that this change does is to remove the crash. - 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. + https://github.com/ceres-solver/ceres-solver/issues/212 - - 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. + Thanks to @NeroBurner and @debalance for reporting this. - Change-Id: I88e2e0001e4586ba2718535845ed1e4b1a5b72bc + Change-Id: I66480e86d4fa0a4b621204f2ff44cc3ff8d01c04 -commit a66d89dcda47cefda83758bfb9e7374bec4ce866 +commit 83041ac84f2d67c28559c67515e0e596a3f3aa20 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sat Sep 5 16:50:20 2015 -0700 +Date: Fri Sep 2 19:10:35 2016 -0700 - Get ready for 1.11.0RC1 + Fix some compiler warnings. - Update version numbers. - Drop CERES_VERSION_ABI macro. + Reported by Richard Trieu. - Change-Id: Ib3eadabb318afe206bb196a5221b195d26cbeaa0 + Change-Id: I202b7a7df09cc19c92582d276ccf171edf88a9fb -commit 1ac3dd223c179fbadaed568ac532af4139c75d84 +commit 8c4623c63a2676e79e7917bb0561f903760f19b9 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sat Sep 5 15:30:01 2015 -0700 +Date: Thu Sep 1 00:05:09 2016 -0700 - Fix a bug in CompressedRowSparseMatrix::AppendRows + Update ExpectArraysClose to use ExpectClose instead of EXPECT_NEAR - 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. + The documentation for ExpectArraysClose and its implementation + did not match. - 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. + This change makes the polynomial_test not fail on 64bit AMD builds. - A pointer to an Eigen matrix was being used as an array. + Thanks to Phillip Huebner for reporting this. - Change-Id: Ifaea14fa3416eda5953de49afb78dc5a6ea816eb + Change-Id: I503f2d3317a28d5885a34f8bdbccd49d20ae9ba2 -commit 5742b7d0f14d2d170054623ccfee09ea214b8ed9 +commit 2fd39fcecb47eebce727081c9ffb8edf86c33669 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Aug 26 09:24:33 2015 -0700 +Date: Thu Sep 1 16:05:06 2016 -0700 - Improve performance of SPARSE_NORMAL_CHOLESKY + dynamic_sparsity + FindWithDefault returns by value rather than reference. + + Returning by reference leads to lifetime issues with the default + value which may go out of scope by the time it is used. + + Thanks to @Ardavel for reporting this, as this causes graph_test + to fail on VS2015x64. - 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. + https://github.com/ceres-solver/ceres-solver/issues/216 - For Eigen and for CXSparse we now explicitly compute the normal - equations using their respective matrix-matrix product routines and solve. - Change-Id: Ifbd8ed78987cdf71640e66ed69500442526a23d4 + Change-Id: I596481219cfbf7622d49a6511ea29193b82c8ba3 -commit d0b6cf657d6ef0dd739e958af9a5768f2eecfd35 -Author: Keir Mierle <mierle@gmail.com> -Date: Fri Sep 4 18:43:41 2015 -0700 +commit 716f049a7b91a8f3a4632c367d9534d1d9190a81 +Author: Mike Vitus <vitus@google.com> +Date: Wed Aug 31 13:38:30 2016 -0700 - Fix incorrect detect structure test + Convert pose graph 2D example to glog and gflags. - Change-Id: I7062f3639147c40b57947790d3b18331a39a366b + Change-Id: I0ed75a60718ef95199bb36f33d9eb99157d11d40 -commit 0e8264cc47661651a11e2dd8570c210082963545 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sat Aug 22 16:23:05 2015 +0100 +commit 46c5ce89dda308088a5fdc238d0c126fdd2c2b58 +Author: David Gossow <dgossow@google.com> +Date: Wed Aug 31 18:40:57 2016 +0200 - Add increased inline threshold (iff Clang) to exported Ceres target. + Fix compiler errors on some systems - - 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. + This fixes some signed-unsigned comparisons and a missing header + include. - Change-Id: I31e8f1abfda140d22e85bb48aa57f028a68a415e + Change-Id: Ieb2bf6e905faa74851bc4ac4658d2f1da24b6ecc -commit a1b3fce9e0a4141b973f6b4dd9b08c4c13052d52 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Mon Aug 31 14:14:56 2015 +0100 +commit b102d53e1dd7dab132e58411183b6fffc2090590 +Author: David Gossow <dgossow@google.com> +Date: Wed Aug 31 10:21:20 2016 +0200 - Add optional export of Ceres build directory to new features list. + Gradient checker multithreading bugfix. - 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. + This is a follow-up on c/7470. GradientCheckingCostFunction calls + callback_->SetGradientErrorDetected() in its Evaluate method, + which will run in multiple threads simultaneously when enabling + this option in the solver. Thus, the string append operation + inside that method has to be protected by a mutex. - Change-Id: I16fe7973534cd556d97215e84268ae0b8ec4e11a + Change-Id: I314ef1df2be52595370d9af05851bf6da39bb45e -commit 01548282cb620e5e3ac79a63a391cd0afd5433e4 +commit 79a28d1e49af53f67af7f3387d07e7c9b7339433 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun Aug 30 22:29:27 2015 -0700 +Date: Wed Aug 31 06:47:45 2016 -0700 - Update the version history. + Rename a confusingly named member of Solver::Options + + Solver::Options::numeric_derivative_relative_step_size to + Solver::Options::gradient_check_numeric_derivative_relative_step_size - Change-Id: I29873bed31675e0108f1a44f53f7bc68976b7f98 + Change-Id: Ib89ae3f87e588d4aba2a75361770d2cec26f07aa -commit 2701429f770fce69ed0c77523fa43d7bc20ac6dc +commit 358ae741c8c4545b03d95c91fa546d9a36683677 Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun Aug 30 21:33:57 2015 -0700 +Date: Wed Aug 31 06:58:41 2016 -0700 - Use Eigen::Dynamic instead of ceres::DYNAMIC in numeric_diff.h + Note that Problem::Evaluate cannot be called from an IterationCallback - Change-Id: Iccb0284a8fb4c2160748dfae24bcd595f1d4cb5c + Change-Id: Ieabdc2d40715e6b547ab22156ba32e9c8444b7ed -commit 4f049db7c2a3ee8cf9910c6eac96be6a28a5999c -Author: Tal Ben-Nun <tbennun@gmail.com> -Date: Wed May 13 15:43:51 2015 +0300 +commit 44044e25b14d7e623baae4505a17c913bdde59f8 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed Aug 31 05:50:58 2016 -0700 - Adaptive numeric differentiation using Ridders' method. + Update the NumTraits for Jets - 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. + 1. Use AVX if EIGEN_VECTORIZE_AVX is defined. + 2. Make the cost of division same as the cost of multiplication. - 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). + These are updates to the original numtraits update needed for eigen 3.3 + that Shaheen Gandhi sent out. - Change-Id: I2d6e924d7ff686650272f29a8c981351e6f72091 + Change-Id: Ic1e3ed7d05a659c7badc79a894679b2dd61c51b9 -commit 070bba4b43b4b7449628bf456a10452fd2b34d28 +commit 4b6ad5d88e45ce8638c882d3e8f08161089b6dba Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Aug 25 13:37:33 2015 -0700 +Date: Sat Aug 27 23:21:55 2016 -0700 - Lint fixes from William Rucklidge + Use ProductParameterization in bundle_adjuster.cc - 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 using a quaternion to parameterize the camera + orientation, the camera parameter block was split into two + parameter blocks. One for the rotation and another for the + translation and intrinsics. This was to enable the use of the + Quaternion parameterization. - - 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). + Now that we have a ProductParameterization which allows us + to compose multiple parameterizations, this is no longer needed + and we use a size 10 parameter block instead. - [1] https://github.com/Itseez/opencv_contrib/pull/290#issuecomment-130389471 + This leads to a more than 2x improvements in the linear solver time. - Change-Id: I99388f1784ece688f91b162d009578c5c97ddaf6 + Change-Id: I78b8f06696f81fee54cfe1a4ae193ee8a5f8e920 -commit 860bba588b981a5718f6b73e7e840e5b8757fe65 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Aug 25 09:43:21 2015 -0700 +commit bfc916cf1cf753b85c1e2ac037e2019ee891f6f9 +Author: Shaheen Gandhi <visigoth@gmail.com> +Date: Thu Aug 4 12:10:14 2016 -0700 - Fix a bug in DetectStructure + Allow ceres to be used with the latest version of Eigen - 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 + Change-Id: Ief3b0f6b405484ec04ecd9ab6a1e1e5409a594c2 -commit b0cbc0f0b0a22f01724b7b647a4a94db959cc4e4 -Author: Johannes Schönberger <hannesschoenberger@gmail.com> -Date: Thu Aug 20 14:21:30 2015 -0400 +commit edbd48ab502aa418ad9700ee5c3ada5f9268b90a +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Sun Jul 10 14:13:51 2016 +0100 - Reduce memory footprint of SubsetParameterization + Enable support for OpenMP in Clang if detected. + + - Previously we disabled OpenMP if Clang was detected, as it did not + support it. However as of Clang 3.8 (and potentially Xcode 8) OpenMP + is supported. - Change-Id: If113cb4696d5aef3e50eed01fba7a3d4143b7ec8 + Change-Id: Ia39dac9fe746f1fc6310e08553f85f3c37349707 -commit ad2a99777786101411a971e59576ca533a297013 -Author: Sergey Sharybin <sergey.vfx@gmail.com> -Date: Sat Aug 22 11:18:45 2015 +0200 +commit f6df6c05dd83b19fa90044106ebaca40957998ae +Author: Mike Vitus <vitus@google.com> +Date: Thu Aug 18 19:27:43 2016 -0700 - 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. + Add an example for modeling and solving a 3D pose graph SLAM problem. - Change-Id: Ia23ae8dfd20c482cb9cd1301f17edf9a34df3235 + Change-Id: I750ca5f20c495edfee5f60ffedccc5bd8ba2bb37 -commit 4bf3868beca9c17615f72ec03730cddb3676acaa -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun Aug 9 15:24:45 2015 -0700 +commit ac3b8e82175122e38bafaaa9cd419ba3cee11087 +Author: David Gossow <dgossow@google.com> +Date: Fri Apr 29 16:07:11 2016 +0200 - Fix a bug in the Schur eliminator + Gradient checking cleanup and local parameterization bugfix - 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. + Change the Ceres gradient checking API to make is useful for + unit testing, clean up code duplication and fix interaction between + gradient checking and local parameterizations. - 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. + There were two gradient checking implementations, one being used + when using the check_gradients flag in the Solver, the other + being a standalone class. The standalone version was restricted + to cost functions with fixed parameter sizes at compile time, which + is being lifted here. This enables it to be used inside the + GradientCheckingCostFunction as well. - 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. + In addition, this installs new hooks in the Solver to ensure + that Solve will fail if any incorrect gradients are detected. This + way, you can set the check_gradient flags to true and detect + errors in an automated way, instead of just printing error information + to the log. The error log is now also returned in the Solver summary + instead of being printed directly. The user can then decide what to + do with it. The existing hooks for user callbacks are used for + this purpose to keep the internal API changes minimal and non-invasive. - 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. + The last and biggest change is the way the the interaction between + local parameterizations and the gradient checker works. Before, + local parameterizations would be ignored by the checker. However, + if a cost function does not compute its Jacobian along the null + space of the local parameterization, this wil not have any effect + on the solver, but would result in a gradient checker error. + With this change, the Jacobians are multiplied by the Jacobians + of the respective local parameterization and thus being compared + in the tangent space only. - Thanks to Werner Trobin for reporting this bug. + The typical use case for this are quaternion parameters, where + a cost function will typically assume that the quaternion is + always normalized, skipping the correct computation of the Jacobian + along the normal to save computation cost. - Change-Id: I8ae7aabf8eed8c3f9cf74b6c74d632ba44f82581 + Change-Id: I5e1bb97b8a899436cea25101efe5011b0bb13282 -commit 1635ce726078f00264b89d7fb6e76fd1c2796e59 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Aug 19 00:26:02 2015 -0700 +commit d4264ec10d9a270b53b5db86c0245ae8cbd2cf18 +Author: Mike Vitus <vitus@google.com> +Date: Wed Aug 17 13:39:05 2016 -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. + Add a quaternion local parameterization for Eigen's quaternion element convention. - Change-Id: I5cfae222d701dfdb8e1bda7f0b4670a30417aa89 + Change-Id: I7046e8b24805313c5fb6a767de581d0054fcdb83 -commit 4c3f8987e7f0c51fd367cf6d43d7eb879e79589f -Author: Simon Rutishauser <simon.rutishauser@pix4d.com> -Date: Thu Aug 13 11:10:44 2015 +0200 +commit fd7cab65ef30fbc33612220abed52dd5073413c4 +Author: Mike Vitus <vitus@google.com> +Date: Wed Aug 10 09:29:12 2016 -0700 - Add missing CERES_EXPORT to ComposedLoss + Fix typos in the pose graph 2D example. - Change-Id: Id7db388d41bf53e6e5704039040c9d2c6bf4c29c + Change-Id: Ie024ff6b6cab9f2e8011d21121a91931bd987bd1 -commit 1a740cc787b85b883a0703403a99fe49662acb79 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Aug 11 18:08:05 2015 -0700 +commit 375dc348745081f89693607142d8b6744a7fb6b4 +Author: Mike Vitus <vitus@google.com> +Date: Wed Aug 3 18:51:16 2016 -0700 - Add the option to use numeric differentiation to nist and more_garbow_hillstrom + Remove duplicate entry for the NIST example in the docs. - Change-Id: If0a5caef90b524dcf5e2567c5b681987f5459401 + Change-Id: Ic4e8f9b68b77b5235b5c96fe588cc56866dab759 -commit ea667ede5c038d6bf3d1c9ec3dbdc5072d1beec6 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sun Aug 9 16:56:13 2015 +0100 +commit f554681bf22d769abc12dd6d346ef65f9bb22431 +Author: Mike Vitus <vitus@google.com> +Date: Mon Jul 25 18:30:48 2016 -0700 - 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). + Add an example for modeling and solving a 2D pose graph SLAM problem. - Change-Id: I11678a9cbc7a817133c22128da01055a3cb8a26d + Change-Id: Ia89b12af7afa33e7b1b9a68d69cf2a0b53416737 -commit a14ec27fb28ab2e8d7f1c9d88e41101dc6c0aab5 -Author: Richard Stebbing <richie.stebbing@gmail.com> -Date: Fri Aug 7 08:42:03 2015 -0700 +commit e1bcc6e0f51512f43aa7bfb7b0d62f7ac1d0cd4b +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed May 18 07:52:48 2016 -0700 - Fix SparseNormalCholeskySolver with dynamic sparsity. - - The previous implementation incorrectly cached the outer product matrix - pattern even when `dynamic_sparsity = true`. + Add additional logging for analyzing orderings - Change-Id: I1e58315a9b44f2f457d07c56b203ab2668bfb8a2 + Change-Id: Ic68d2959db35254e2895f11294fb25de4d4b8a81 -commit 3dd7fced44ff00197fa9fcb1f2081d12be728062 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sun Aug 9 16:38:50 2015 +0100 +commit 16980b4fec846f86910c18772b8145bcb55f4728 +Author: Mike Vitus <vitus@google.com> +Date: Fri Jul 15 13:37:49 2016 -0700 - Remove legacy dependency detection macros. + Delete the remove_definitons command from sampled_functions + CMakeLists.txt because it will be inherited from the top level examples + CMakeLists.txt. - - 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 + Change-Id: I25593587df0ae84fd8ddddc589bc2a13f3777427 -commit 8b32e258ccce1eed2a50bb002add16cad13aff1e -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sun Aug 9 15:42:39 2015 +0100 +commit a04490be97800e78e59db5eb67fa46226738ffba +Author: Mike Vitus <vitus@google.com> +Date: Thu Jul 14 10:10:13 2016 -0700 - 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. + Add readme for the sampled_function example. - Change-Id: Ia92e97523d7a1478ab36539726b9540d7cfee5d0 + Change-Id: I9468b6a7b9f2ffdd2bf9f0dd1f4e1d5f894e540c -commit cc8d47aabb9d63ba4588ba7295058a6191c2df83 +commit ff11d0e63d4678188e8cabd40a532ba06912fe5a Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sun Aug 9 15:18:42 2015 +0100 +Date: Wed Jun 29 09:31:45 2016 +0100 - Update all CMake to lowercase function name style. + Use _j[0,1,n]() Bessel functions on MSVC to avoid deprecation errors. - - 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. + - Microsoft deprecated the POSIX Bessel functions: j[0,1,n]() in favour + of _j[0,1,n](), it appears since at least MSVC 2005: + https://msdn.microsoft.com/en-us/library/ms235384(v=vs.100).aspx. + - As this occurs in jet.h (templated public header), although Ceres + suppresses the warning when it itself is built (to suppress a warning + about the insecurity of using std::copy), it will crop up again in + client code (without this fix) unless it is explicitly suppressed + there also. + - Raised as Issue #190: + https://github.com/ceres-solver/ceres-solver/issues/190. - Change-Id: If7219816f560270e59212813aeb021353a64a0e2 + Change-Id: If7ac5dbb856748f9900be93ec0452a40c0b00524 -commit 1f106904c1f47460c35ac03258d6506bb2d60838 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sun Aug 9 14:55:02 2015 +0100 +commit 8ea86e1614cf77644ce782e43cde6565a54444f5 +Author: Nicolai Wojke <nwojke@uni-koblenz.de> +Date: Mon Apr 25 14:24:41 2016 +0200 - 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+. + Fix: Copy minimizer option 'is_silent' to LinSearchDirection::Options - Change-Id: Iefec8f03408b8cdc7a495f442ebba081f800adb0 + Change-Id: I23b4c3383cad30033c539ac93883d77c8dd4ba1a -commit 16ecd40523a408e7705c9fdb0e159cef2007b8ab -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sat Aug 8 17:32:31 2015 +0100 +commit 080ca4c5f2ac42620971a07f06d2d13deb7befa8 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun Apr 24 22:46:54 2016 -0700 - 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. + Fix typos in users.rst - Change-Id: Iceccc3ee53c7c2010e41cc45255f966e7b13d526 + Change-Id: Ifdc67638a39403354bc9589f42a1b42cb9984dd2 -commit 56be8de007dfd65ed5a31c795eb4a08ad765f411 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Thu Jun 25 21:31:00 2015 +0100 +commit 21ab397dc55335c147fdd795899b1f8981037b09 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun Apr 24 21:13:00 2016 -0700 - 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. + Make some Jet comparisons exact. - Change-Id: I38925ae3bb8c16682d404468ba95c611a519b9b9 + Change-Id: Ia08c72f3b8779df96f5c0d5a954b2c0a1dd3a061 -commit cf863b6415ac4dbf3626e70adeac1ac0f3d87ee5 +commit ee40f954cf464087eb8943abf4d9db8917a33fbe Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu Aug 6 14:52:18 2015 -0700 +Date: Sun Apr 24 07:49:55 2016 -0700 - Remove the spec file needed for generating RPMs. + Add colmap to users.rst - Now that ceres is part of RawHide, there is no need to carry - this spec file with the ceres distribution. - - Change-Id: Icc400b9874ba05ba05b353e2658f1de94c72299e + Change-Id: I452a8c1dc6a3bc55734b2fc3a4002ff7939ba863 -commit 560940fa277a469c1ab34f1aa303ff1af9c3cacf +commit 9665e099022bd06e53b0779550e9aebded7f274d Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sat Jul 11 22:21:31 2015 -0700 +Date: Mon Apr 18 06:00:58 2016 -0700 - A refactor of the cubic interpolation code + Fix step norm evaluation in LineSearchMinimizer - 1. Push the boundary handling logic into the underlying array - object. This has two very significant impacts: + TrustRegionMinimizer evaluates the size of the step + taken in the ambient space, where as the LineSearchMinimizer + was using the norm in the tangent space. This change fixes + this discrepancy. - a. The interpolation code becomes extremely simple to write - and to test. + Change-Id: I9fef64cbb5622c9769c0413003cfb1dc6e89cfa3 + +commit 620ca9d0668cd4a00402264fddca3cf6bd2e7265 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Mon Apr 18 15:14:11 2016 +0100 + + Remove use of -Werror when compiling Ceres. - b. The user has more flexibility in implementing how out of bounds - values are handled. We provide one default implementation. + - As noted in Issue #193 (in that case for GCC 6), Ceres' use of -Werror + when compiling on *nix can prevent compilation on new compilers that + add new warnings and there is an inevitable delay between new compiler + versions and Ceres versions. + - Removing the explicit use of -Werror, and relying on indirect + verification by maintainers should fix build issues for Ceres releases + on newer compilers. - Change-Id: Ic2f6cf9257ce7110c62e492688e5a6c8be1e7df2 + Change-Id: I38e9ade28d4a90e53dcd918a7d470f1a1debd7b4 -commit dfdf19e111c2b0e6daeb6007728ec2f784106d49 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Aug 5 15:20:57 2015 -0700 +commit 0c63bd3efbf1d41151c9fab41d4b77dc64c572c8 +Author: Mike Vitus <vitus@google.com> +Date: Thu Apr 14 10:25:52 2016 -0700 - Lint cleanup from Jim Roseborough + Add floor and ceil functions to the Jet implementation. - Change-Id: Id6845c85644d40e635ed196ca74fc51a387aade4 + Change-Id: I72ebfb0e9ade2964dbf3a014225ead345d5ae352 -commit 7444f23ae245476a7ac8421cc2f88d6947fd3e5f -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Aug 3 12:22:44 2015 -0700 +commit 9843f3280356c158d23c06a16085c6c5ba35e053 +Author: Alex Stewart <alexs.mac@gmail.com> +Date: Mon Mar 7 21:24:32 2016 +0000 - 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. + Report Ceres compile options as components in find_package(). - Thanks to Werner Trobin for pointing this out. + - Users can now specify particular components from Ceres, such as + SuiteSparse support) that must be present in a detected version of + Ceres in order for it to be reported as found by find_package(). + - This allows users to specify for example that they require a version + of Ceres with SuiteSparse support at configure time, rather than + finding out only at run time that Ceres was not compiled with the + options they require. + - The list of available components are built directly from the Ceres + compile options. + - The meta-module SparseLinearAlgebraLibrary is present if at least + one sparse linear algebra backend is available. - Change-Id: Id2b4d9326ec21baec8a85423e3270aefbafb611e + Change-Id: I65f1ddfd7697e6dd25bb4ac7e54f5097d3ca6266 -commit 5a48b92123b30a437f031eb24b0deaadc8f60d26 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sat Jul 4 17:59:52 2015 +0100 +commit e4d4d88bbe51b9cc0f7450171511abbea0779790 +Author: Timer <linyicx@126.com> +Date: Fri Apr 8 15:42:18 2016 +0800 - 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. + Fix a spelling error in nnls_modeling.rst - Change-Id: Ib5a7588446f490e1b405878475b6b1dd13accd1f + Change-Id: I341d901d3df993bc5397ed15e6cb330b0c38fd72 -commit d9790e77894ea99d38137d359d6118315b2d1601 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun Jul 12 19:39:47 2015 -0700 +commit 5512f58536e1be0d92010d8325b606e7b4733a08 +Author: Keir Mierle <mierle@gmail.com> +Date: Thu Apr 7 12:03:16 2016 -0700 - Add ProductParameterization + Only use collapse() directive with OpenMP 3.0 or higher - 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: Icba544c0494763c57eb6dc61e98379312ca15972 + +commit d61e94da5225217cab7b4f93b72f97055094681f +Author: Thomas Schneider <schneith@ethz.ch> +Date: Wed Apr 6 10:40:29 2016 +0200 + + Add IsParameterBlockConstant to the ceres::Problem class. - Change-Id: I4b5bcbd2407a38739c7725b129789db5c3d65a20 + Change-Id: I7d0e828e81324443209c17fa54dd1d37605e5bfe -commit 7b4fb69dad49eaefb5d2d47ef0d76f48ad7fef73 +commit 77d94b34741574e958a417561702d6093fba87fb 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 +Date: Sun Feb 14 16:54:03 2016 +0000 - Add default glog install location on Windows to search paths. + Fix install path for CeresConfig.cmake to be architecture-aware. + + - Previously we were auto-detecting a "64" suffix for the install path + for the Ceres library on non-Debian/Arch Linux distributions, but + we were installing CeresConfig.cmake to an architecture independent + location. + - We now install CeresConfig.cmake to lib${LIB_SUFFIX}/cmake/Ceres. + - Also make LIB_SUFFIX visible to the user in the CMake GUI s/t they can + easily override the auto-detected value if desired. + - Reported by jpgr87@gmail.com as Issue #194. - Change-Id: I083d368be48986e6780c11460f5a07b2f3b6c900 + Change-Id: If126260d7af685779487c01220ae178ac31f7aea diff --git a/extern/ceres/bundle.sh b/extern/ceres/bundle.sh index 0eaf00f3989..a4f703ac33d 100755 --- a/extern/ceres/bundle.sh +++ b/extern/ceres/bundle.sh @@ -173,26 +173,5 @@ if(WITH_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/files.txt b/extern/ceres/files.txt index f49f1fb0ded..4d973bbcdc2 100644 --- a/extern/ceres/files.txt +++ b/extern/ceres/files.txt @@ -149,6 +149,7 @@ 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_checker.cc internal/ceres/gradient_checking_cost_function.cc internal/ceres/gradient_checking_cost_function.h internal/ceres/gradient_problem.cc @@ -160,6 +161,8 @@ internal/ceres/householder_vector.h internal/ceres/implicit_schur_complement.cc internal/ceres/implicit_schur_complement.h internal/ceres/integral_types.h +internal/ceres/is_close.cc +internal/ceres/is_close.h internal/ceres/iterative_schur_complement_solver.cc internal/ceres/iterative_schur_complement_solver.h internal/ceres/lapack.cc @@ -243,6 +246,8 @@ 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_step_evaluator.cc +internal/ceres/trust_region_step_evaluator.h internal/ceres/trust_region_strategy.cc internal/ceres/trust_region_strategy.h internal/ceres/types.cc diff --git a/extern/ceres/include/ceres/cost_function_to_functor.h b/extern/ceres/include/ceres/cost_function_to_functor.h index 6c67ac0f937..d2dc94725e4 100644 --- a/extern/ceres/include/ceres/cost_function_to_functor.h +++ b/extern/ceres/include/ceres/cost_function_to_functor.h @@ -130,7 +130,8 @@ class CostFunctionToFunctor { 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(static_cast<int>(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 diff --git a/extern/ceres/include/ceres/covariance.h b/extern/ceres/include/ceres/covariance.h index dd20dc36ba1..930f96cf3ae 100644 --- a/extern/ceres/include/ceres/covariance.h +++ b/extern/ceres/include/ceres/covariance.h @@ -357,6 +357,28 @@ class CERES_EXPORT Covariance { const double*> >& covariance_blocks, Problem* problem); + // Compute a part of the covariance matrix. + // + // The vector parameter_blocks contains the parameter blocks that + // are used for computing the covariance matrix. From this vector + // all covariance pairs are generated. This allows the covariance + // estimation algorithm to only compute and store these blocks. + // + // parameter_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<const double*>& parameter_blocks, + Problem* problem); + // Return the block of the cross-covariance matrix corresponding to // parameter_block1 and parameter_block2. // @@ -394,6 +416,40 @@ class CERES_EXPORT Covariance { const double* parameter_block2, double* covariance_block) const; + // Return the covariance matrix corresponding to all parameter_blocks. + // + // Compute must be called before calling GetCovarianceMatrix and all + // parameter_blocks must have been present in the vector + // parameter_blocks when Compute was called. Otherwise + // GetCovarianceMatrix returns false. + // + // covariance_matrix must point to a memory location that can store + // the size of the covariance matrix. The covariance matrix will be + // a square matrix whose row and column count is equal to the sum of + // the sizes of the individual parameter blocks. The covariance + // matrix will be a row-major matrix. + bool GetCovarianceMatrix(const std::vector<const double *> ¶meter_blocks, + double *covariance_matrix); + + // Return the covariance matrix corresponding to parameter_blocks + // in the tangent space if a local parameterization is associated + // with one of the parameter blocks else returns the covariance + // matrix in the ambient space. + // + // Compute must be called before calling GetCovarianceMatrix and all + // parameter_blocks must have been present in the vector + // parameters_blocks when Compute was called. Otherwise + // GetCovarianceMatrix returns false. + // + // covariance_matrix must point to a memory location that can store + // the size of the covariance matrix. The covariance matrix will be + // a square matrix whose row and column count is equal to the sum of + // the sizes of the tangent spaces of the individual parameter + // blocks. The covariance matrix will be a row-major matrix. + bool GetCovarianceMatrixInTangentSpace( + const std::vector<const double*>& parameter_blocks, + double* covariance_matrix); + private: internal::scoped_ptr<internal::CovarianceImpl> impl_; }; diff --git a/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h b/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h index c852d57a3fc..5770946a115 100644 --- a/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h +++ b/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h @@ -85,22 +85,6 @@ class DynamicNumericDiffCostFunction : public CostFunction { 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(); @@ -138,19 +122,19 @@ class DynamicNumericDiffCostFunction : public CostFunction { 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) { + for (size_t 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) { + for (size_t 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) { + for (size_t block = 0; block < block_sizes.size(); ++block) { if (jacobians[block] != NULL && !NumericDiff<CostFunctor, method, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, diff --git a/extern/ceres/include/ceres/gradient_checker.h b/extern/ceres/include/ceres/gradient_checker.h index 28304159b44..6d285daf1d9 100644 --- a/extern/ceres/include/ceres/gradient_checker.h +++ b/extern/ceres/include/ceres/gradient_checker.h @@ -27,194 +27,121 @@ // 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. +// Authors: wjr@google.com (William Rucklidge), +// keir@google.com (Keir Mierle), +// dgossow@google.com (David Gossow) #ifndef CERES_PUBLIC_GRADIENT_CHECKER_H_ #define CERES_PUBLIC_GRADIENT_CHECKER_H_ -#include <cstddef> -#include <algorithm> #include <vector> +#include <string> +#include "ceres/cost_function.h" +#include "ceres/dynamic_numeric_diff_cost_function.h" #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 "ceres/local_parameterization.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. +// GradientChecker compares the Jacobians returned by a cost function against +// derivatives estimated using finite differencing. // -// The only likely usage of this is for testing. +// The condition enforced is that // -// 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. +// (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) multiplied by the local parameterization Jacobian +// and J_numeric is the jacobian as computed by finite differences, multiplied +// by the local parameterization Jacobian as well. // -// 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> +// 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'. 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. + // This will not take ownership of the cost function or local + // parameterizations. + // + // function: The cost function to probe. + // local_parameterization: A vector of local parameterizations for each + // parameter. May be NULL or contain NULL pointers to indicate that the + // respective parameter does not have a local parameterization. + // options: Options to use for numerical differentiation. + GradientChecker( + const CostFunction* function, + const std::vector<const LocalParameterization*>* local_parameterizations, + const NumericDiffOptions& options); + + // Contains results from a call to Probe for later inspection. + struct ProbeResults { + // The return value of the cost function. + bool return_value; + + // Computed residual vector. + Vector residuals; + + // The sizes of the Jacobians below are dictated by the cost function's + // parameter block size and residual block sizes. If a parameter block + // has a local parameterization associated with it, the size of the "local" + // Jacobian will be determined by the local parameterization dimension and + // residual block size, otherwise it will be identical to the regular + // Jacobian. // Derivatives as computed by the cost function. - std::vector<Matrix> term_jacobians; + std::vector<Matrix> jacobians; + + // Derivatives as computed by the cost function in local space. + std::vector<Matrix> local_jacobians; - // Derivatives as computed by finite differencing. - std::vector<Matrix> finite_difference_jacobians; + // Derivatives as computed by nuerical differentiation in local space. + std::vector<Matrix> numeric_jacobians; - // Infinity-norm of term_jacobians - finite_difference_jacobians. - double error_jacobians; + // Derivatives as computed by nuerical differentiation in local space. + std::vector<Matrix> local_numeric_jacobians; + + // Contains the maximum relative error found in the local Jacobians. + double maximum_relative_error; + + // If an error was detected, this will contain a detailed description of + // that error. + std::string error_log; }; - // 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. + // Call the cost function, compute alternative Jacobians using finite + // differencing and compare results. If local parameterizations are given, + // the Jacobians will be multiplied by the local parameterization Jacobians + // before performing the check, which effectively means that all errors along + // the null space of the local parameterization will be ignored. + // Returns false if the Jacobians don't match, the cost function return false, + // or if the cost function returns different residual when called with a + // Jacobian output argument vs. calling it without. Otherwise returns true. // - // 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. + // parameters: The parameter values at which to probe. + // relative_precision: A threshold for the relative difference between the + // Jacobians. If the Jacobians differ by more than this amount, then the + // probe fails. + // results: On return, the 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; - } + bool Probe(double const* const* parameters, + double relative_precision, + ProbeResults* results) const; private: CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(GradientChecker); + + std::vector<const LocalParameterization*> local_parameterizations_; + const CostFunction* function_; + internal::scoped_ptr<CostFunction> finite_diff_cost_function_; }; } // namespace ceres diff --git a/extern/ceres/include/ceres/internal/port.h b/extern/ceres/include/ceres/internal/port.h index e57049dde4b..f4dcaee7bd8 100644 --- a/extern/ceres/include/ceres/internal/port.h +++ b/extern/ceres/include/ceres/internal/port.h @@ -33,9 +33,8 @@ // This file needs to compile as c code. #ifdef __cplusplus - +#include <cstddef> #include "ceres/internal/config.h" - #if defined(CERES_TR1_MEMORY_HEADER) #include <tr1/memory> #else @@ -50,6 +49,25 @@ using std::tr1::shared_ptr; using std::shared_ptr; #endif +// We allocate some Eigen objects on the stack and other places they +// might not be aligned to 16-byte boundaries. If we have C++11, we +// can specify their alignment anyway, and thus can safely enable +// vectorization on those matrices; in C++99, we are out of luck. Figure out +// what case we're in and write macros that do the right thing. +#ifdef CERES_USE_CXX11 +namespace port_constants { +static constexpr size_t kMaxAlignBytes = + // Work around a GCC 4.8 bug + // (https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56019) where + // std::max_align_t is misplaced. +#if defined (__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ == 8 + alignof(::max_align_t); +#else + alignof(std::max_align_t); +#endif +} // namespace port_constants +#endif + } // namespace ceres #endif // __cplusplus diff --git a/extern/ceres/include/ceres/iteration_callback.h b/extern/ceres/include/ceres/iteration_callback.h index 6bab00439c5..db5d0efe53a 100644 --- a/extern/ceres/include/ceres/iteration_callback.h +++ b/extern/ceres/include/ceres/iteration_callback.h @@ -69,7 +69,7 @@ struct CERES_EXPORT IterationSummary { // 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. + // Note: step_is_valid is always true when iteration = 0. bool step_is_valid; // Step did not reduce the value of the objective function @@ -77,7 +77,7 @@ struct CERES_EXPORT IterationSummary { // acceptance criterion used by the non-monotonic trust region // algorithm. // - // Note: step_is_nonmonotonic is false when iteration = 0; + // Note: step_is_nonmonotonic is always false when iteration = 0; bool step_is_nonmonotonic; // Whether or not the minimizer accepted this step or not. If the @@ -89,7 +89,7 @@ struct CERES_EXPORT IterationSummary { // 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. + // Note: step_is_successful is always true when iteration = 0. bool step_is_successful; // Value of the objective function. diff --git a/extern/ceres/include/ceres/jet.h b/extern/ceres/include/ceres/jet.h index a21fd7adb90..a104707298c 100644 --- a/extern/ceres/include/ceres/jet.h +++ b/extern/ceres/include/ceres/jet.h @@ -164,6 +164,7 @@ #include "Eigen/Core" #include "ceres/fpclassify.h" +#include "ceres/internal/port.h" namespace ceres { @@ -227,21 +228,23 @@ struct Jet { 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. + + // We allocate Jets on the stack and other places they + // might not be aligned to 16-byte boundaries. If we have C++11, we + // can specify their alignment anyway, and thus can safely enable + // vectorization on those matrices; in C++99, we are out of luck. Figure out + // what case we're in and do the right thing. +#ifndef CERES_USE_CXX11 + // fall back to safe version: Eigen::Matrix<T, N, 1, Eigen::DontAlign> v; +#else + static constexpr bool kShouldAlignMatrix = + 16 <= ::ceres::port_constants::kMaxAlignBytes; + static constexpr int kAlignHint = kShouldAlignMatrix ? + Eigen::AutoAlign : Eigen::DontAlign; + static constexpr size_t kAlignment = kShouldAlignMatrix ? 16 : 1; + alignas(kAlignment) Eigen::Matrix<T, N, 1, kAlignHint> v; +#endif }; // Unary + @@ -388,6 +391,8 @@ 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 floor (double x) { return std::floor(x); } +inline double ceil (double x) { return std::ceil(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); } @@ -482,10 +487,51 @@ Jet<T, N> tanh(const Jet<T, N>& f) { return Jet<T, N>(tanh_a, tmp * f.v); } +// The floor function should be used with extreme care as this operation will +// result in a zero derivative which provides no information to the solver. +// +// floor(a + h) ~= floor(a) + 0 +template <typename T, int N> inline +Jet<T, N> floor(const Jet<T, N>& f) { + return Jet<T, N>(floor(f.a)); +} + +// The ceil function should be used with extreme care as this operation will +// result in a zero derivative which provides no information to the solver. +// +// ceil(a + h) ~= ceil(a) + 0 +template <typename T, int N> inline +Jet<T, N> ceil(const Jet<T, N>& f) { + return Jet<T, N>(ceil(f.a)); +} + // 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); } +// +// Microsoft has deprecated the j[0,1,n]() POSIX Bessel functions in favour of +// _j[0,1,n](). Where available on MSVC, use _j[0,1,n]() to avoid deprecated +// function errors in client code (the specific warning is suppressed when +// Ceres itself is built). +inline double BesselJ0(double x) { +#if defined(_MSC_VER) && defined(_j0) + return _j0(x); +#else + return j0(x); +#endif +} +inline double BesselJ1(double x) { +#if defined(_MSC_VER) && defined(_j1) + return _j1(x); +#else + return j1(x); +#endif +} +inline double BesselJn(int n, double x) { +#if defined(_MSC_VER) && defined(_jn) + return _jn(n, x); +#else + return jn(n, x); +#endif +} // For the formulae of the derivatives of the Bessel functions see the book: // Olver, Lozier, Boisvert, Clark, NIST Handbook of Mathematical Functions, @@ -743,7 +789,15 @@ template<typename T, int N> inline Jet<T, N> ei_pow (const Jet<T, N>& x, // 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() << "]"; + s << "[" << z.a << " ; "; + for (int i = 0; i < N; ++i) { + s << z.v[i]; + if (i != N - 1) { + s << ", "; + } + } + s << "]"; + return s; } } // namespace ceres @@ -757,6 +811,7 @@ struct NumTraits<ceres::Jet<T, N> > { typedef ceres::Jet<T, N> Real; typedef ceres::Jet<T, N> NonInteger; typedef ceres::Jet<T, N> Nested; + typedef ceres::Jet<T, N> Literal; static typename ceres::Jet<T, N> dummy_precision() { return ceres::Jet<T, N>(1e-12); @@ -777,6 +832,21 @@ struct NumTraits<ceres::Jet<T, N> > { HasFloatingPoint = 1, RequireInitialization = 1 }; + + template<bool Vectorized> + struct Div { + enum { +#if defined(EIGEN_VECTORIZE_AVX) + AVX = true, +#else + AVX = false, +#endif + + // Assuming that for Jets, division is as expensive as + // multiplication. + Cost = 3 + }; + }; }; } // namespace Eigen diff --git a/extern/ceres/include/ceres/local_parameterization.h b/extern/ceres/include/ceres/local_parameterization.h index 67633de309f..379fc684921 100644 --- a/extern/ceres/include/ceres/local_parameterization.h +++ b/extern/ceres/include/ceres/local_parameterization.h @@ -211,6 +211,28 @@ class CERES_EXPORT QuaternionParameterization : public LocalParameterization { virtual int LocalSize() const { return 3; } }; +// Implements the quaternion local parameterization for Eigen's representation +// of the quaternion. Eigen uses a different internal memory layout for the +// elements of the quaternion than what is commonly used. Specifically, Eigen +// stores the elements in memory as [x, y, z, w] where the real part is last +// whereas it is typically stored first. Note, when creating an Eigen quaternion +// through the constructor the elements are accepted in w, x, y, z order. Since +// Ceres operates on parameter blocks which are raw double pointers this +// difference is important and requires a different parameterization. +// +// Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x +// with * being the quaternion multiplication operator. +class EigenQuaternionParameterization : public ceres::LocalParameterization { + public: + virtual ~EigenQuaternionParameterization() {} + 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 diff --git a/extern/ceres/include/ceres/numeric_diff_cost_function.h b/extern/ceres/include/ceres/numeric_diff_cost_function.h index fa96078df02..5dfaeab6241 100644 --- a/extern/ceres/include/ceres/numeric_diff_cost_function.h +++ b/extern/ceres/include/ceres/numeric_diff_cost_function.h @@ -206,29 +206,6 @@ class NumericDiffCostFunction } } - // 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(); diff --git a/extern/ceres/include/ceres/problem.h b/extern/ceres/include/ceres/problem.h index 409274c62c2..27ed4ef15da 100644 --- a/extern/ceres/include/ceres/problem.h +++ b/extern/ceres/include/ceres/problem.h @@ -309,6 +309,9 @@ class CERES_EXPORT Problem { // Allow the indicated parameter block to vary during optimization. void SetParameterBlockVariable(double* values); + // Returns true if a parameter block is set constant, and false otherwise. + bool IsParameterBlockConstant(double* values) const; + // 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 @@ -461,6 +464,10 @@ class CERES_EXPORT Problem { // parameter block has a local parameterization, then it contributes // "LocalSize" entries to the gradient vector (and the number of // columns in the jacobian). + // + // Note 3: This function cannot be called while the problem is being + // solved, for example it cannot be called from an IterationCallback + // at the end of an iteration during a solve. bool Evaluate(const EvaluateOptions& options, double* cost, std::vector<double>* residuals, diff --git a/extern/ceres/include/ceres/rotation.h b/extern/ceres/include/ceres/rotation.h index e9496d772e4..b6a06f772c4 100644 --- a/extern/ceres/include/ceres/rotation.h +++ b/extern/ceres/include/ceres/rotation.h @@ -48,7 +48,6 @@ #include <algorithm> #include <cmath> #include <limits> -#include "glog/logging.h" namespace ceres { @@ -418,7 +417,6 @@ 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)); } @@ -496,7 +494,6 @@ void QuaternionToRotation(const T q[4], 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) { diff --git a/extern/ceres/include/ceres/solver.h b/extern/ceres/include/ceres/solver.h index 318cf48cb83..0d77d242dfe 100644 --- a/extern/ceres/include/ceres/solver.h +++ b/extern/ceres/include/ceres/solver.h @@ -134,7 +134,7 @@ class CERES_EXPORT Solver { trust_region_problem_dump_format_type = TEXTFILE; check_gradients = false; gradient_check_relative_precision = 1e-8; - numeric_derivative_relative_step_size = 1e-6; + gradient_check_numeric_derivative_relative_step_size = 1e-6; update_state_every_iteration = false; } @@ -701,12 +701,22 @@ class CERES_EXPORT Solver { // 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: + // WARNING: This option only applies to the to the numeric + // differentiation used for checking the user provided derivatives + // when when Solver::Options::check_gradients is true. If you are + // using NumericDiffCostFunction and are interested in changing + // the step size for numeric differentiation in your cost + // function, please have a look at + // include/ceres/numeric_diff_options.h. // - // delta = numeric_derivative_relative_step_size; + // Relative shift used for taking numeric derivatives when + // Solver::Options::check_gradients is true. + // + // For finite differencing, each dimension is evaluated at + // slightly shifted values; for the case of central difference, + // this is what gets evaluated: + // + // delta = gradient_check_numeric_derivative_relative_step_size; // f_initial = f(x) // f_forward = f((1 + delta) * x) // f_backward = f((1 - delta) * x) @@ -723,7 +733,7 @@ class CERES_EXPORT Solver { // 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; + double gradient_check_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 @@ -801,6 +811,13 @@ class CERES_EXPORT Solver { // Number of times inner iterations were performed. int num_inner_iteration_steps; + // Total number of iterations inside the line search algorithm + // across all invocations. We call these iterations "steps" to + // distinguish them from the outer iterations of the line search + // and trust region minimizer algorithms which call the line + // search algorithm as a subroutine. + int num_line_search_steps; + // All times reported below are wall times. // When the user calls Solve, before the actual optimization diff --git a/extern/ceres/include/ceres/version.h b/extern/ceres/include/ceres/version.h index 66505a515c9..2f1cc297a38 100644 --- a/extern/ceres/include/ceres/version.h +++ b/extern/ceres/include/ceres/version.h @@ -32,7 +32,7 @@ #define CERES_PUBLIC_VERSION_H_ #define CERES_VERSION_MAJOR 1 -#define CERES_VERSION_MINOR 11 +#define CERES_VERSION_MINOR 12 #define CERES_VERSION_REVISION 0 // Classic CPP stringifcation; the extra level of indirection allows the diff --git a/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc index 64b6ac00447..40977b74c67 100644 --- a/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc +++ b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc @@ -46,6 +46,7 @@ namespace internal { using std::make_pair; using std::pair; using std::vector; +using std::adjacent_find; void CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors( const Program* program, CompressedRowSparseMatrix* jacobian) { @@ -140,12 +141,21 @@ SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { // 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."; + if (adjacent_find(parameter_indices.begin(), parameter_indices.end()) != + parameter_indices.end()) { + std::string parameter_block_description; + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + parameter_block_description += + parameter_block->ToString() + "\n"; + } + LOG(FATAL) << "Ceres internal error: " + << "Duplicate parameter blocks detected in a cost function. " + << "This should never happen. Please report this to " + << "the Ceres developers.\n" + << "Residual Block: " << residual_block->ToString() << "\n" + << "Parameter Blocks: " << parameter_block_description; + } // Update the row indices. const int num_residuals = residual_block->NumResiduals(); diff --git a/extern/ceres/internal/ceres/covariance.cc b/extern/ceres/internal/ceres/covariance.cc index 690847945a9..cb280a36847 100644 --- a/extern/ceres/internal/ceres/covariance.cc +++ b/extern/ceres/internal/ceres/covariance.cc @@ -38,6 +38,7 @@ namespace ceres { +using std::make_pair; using std::pair; using std::vector; @@ -54,6 +55,12 @@ bool Covariance::Compute( return impl_->Compute(covariance_blocks, problem->problem_impl_.get()); } +bool Covariance::Compute( + const vector<const double*>& parameter_blocks, + Problem* problem) { + return impl_->Compute(parameter_blocks, problem->problem_impl_.get()); +} + bool Covariance::GetCovarianceBlock(const double* parameter_block1, const double* parameter_block2, double* covariance_block) const { @@ -73,4 +80,20 @@ bool Covariance::GetCovarianceBlockInTangentSpace( covariance_block); } +bool Covariance::GetCovarianceMatrix( + const vector<const double*>& parameter_blocks, + double* covariance_matrix) { + return impl_->GetCovarianceMatrixInTangentOrAmbientSpace(parameter_blocks, + true, // ambient + covariance_matrix); +} + +bool Covariance::GetCovarianceMatrixInTangentSpace( + const std::vector<const double *>& parameter_blocks, + double *covariance_matrix) { + return impl_->GetCovarianceMatrixInTangentOrAmbientSpace(parameter_blocks, + false, // tangent + covariance_matrix); +} + } // namespace ceres diff --git a/extern/ceres/internal/ceres/covariance_impl.cc b/extern/ceres/internal/ceres/covariance_impl.cc index 3e8302bed55..d698f88fa9b 100644 --- a/extern/ceres/internal/ceres/covariance_impl.cc +++ b/extern/ceres/internal/ceres/covariance_impl.cc @@ -36,6 +36,8 @@ #include <algorithm> #include <cstdlib> +#include <numeric> +#include <sstream> #include <utility> #include <vector> @@ -43,6 +45,7 @@ #include "Eigen/SparseQR" #include "Eigen/SVD" +#include "ceres/collections_port.h" #include "ceres/compressed_col_sparse_matrix_utils.h" #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/covariance.h" @@ -51,6 +54,7 @@ #include "ceres/map_util.h" #include "ceres/parameter_block.h" #include "ceres/problem_impl.h" +#include "ceres/residual_block.h" #include "ceres/suitesparse.h" #include "ceres/wall_time.h" #include "glog/logging.h" @@ -61,6 +65,7 @@ namespace internal { using std::make_pair; using std::map; using std::pair; +using std::sort; using std::swap; using std::vector; @@ -86,8 +91,38 @@ CovarianceImpl::CovarianceImpl(const Covariance::Options& options) CovarianceImpl::~CovarianceImpl() { } +template <typename T> void CheckForDuplicates(vector<T> blocks) { + sort(blocks.begin(), blocks.end()); + typename vector<T>::iterator it = + std::adjacent_find(blocks.begin(), blocks.end()); + if (it != blocks.end()) { + // In case there are duplicates, we search for their location. + map<T, vector<int> > blocks_map; + for (int i = 0; i < blocks.size(); ++i) { + blocks_map[blocks[i]].push_back(i); + } + + std::ostringstream duplicates; + while (it != blocks.end()) { + duplicates << "("; + for (int i = 0; i < blocks_map[*it].size() - 1; ++i) { + duplicates << blocks_map[*it][i] << ", "; + } + duplicates << blocks_map[*it].back() << ")"; + it = std::adjacent_find(it + 1, blocks.end()); + if (it < blocks.end()) { + duplicates << " and "; + } + } + + LOG(FATAL) << "Covariance::Compute called with duplicate blocks at " + << "indices " << duplicates.str(); + } +} + bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks, ProblemImpl* problem) { + CheckForDuplicates<pair<const double*, const double*> >(covariance_blocks); problem_ = problem; parameter_block_to_row_index_.clear(); covariance_matrix_.reset(NULL); @@ -97,6 +132,20 @@ bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks, return is_valid_; } +bool CovarianceImpl::Compute(const vector<const double*>& parameter_blocks, + ProblemImpl* problem) { + CheckForDuplicates<const double*>(parameter_blocks); + CovarianceBlocks covariance_blocks; + for (int i = 0; i < parameter_blocks.size(); ++i) { + for (int j = i; j < parameter_blocks.size(); ++j) { + covariance_blocks.push_back(make_pair(parameter_blocks[i], + parameter_blocks[j])); + } + } + + return Compute(covariance_blocks, problem); +} + bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace( const double* original_parameter_block1, const double* original_parameter_block2, @@ -120,9 +169,17 @@ bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace( 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(); + const int block1_local_size = block1->LocalSize(); + const int block2_local_size = block2->LocalSize(); + if (!lift_covariance_to_ambient_space) { + MatrixRef(covariance_block, block1_local_size, block2_local_size) + .setZero(); + } else { + MatrixRef(covariance_block, block1_size, block2_size).setZero(); + } return true; } @@ -240,6 +297,94 @@ bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace( return true; } +bool CovarianceImpl::GetCovarianceMatrixInTangentOrAmbientSpace( + const vector<const double*>& parameters, + bool lift_covariance_to_ambient_space, + double* covariance_matrix) const { + CHECK(is_computed_) + << "Covariance::GetCovarianceMatrix called before Covariance::Compute"; + CHECK(is_valid_) + << "Covariance::GetCovarianceMatrix called when Covariance::Compute " + << "returned false."; + + const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map(); + // For OpenMP compatibility we need to define these vectors in advance + const int num_parameters = parameters.size(); + vector<int> parameter_sizes; + vector<int> cum_parameter_size; + parameter_sizes.reserve(num_parameters); + cum_parameter_size.resize(num_parameters + 1); + cum_parameter_size[0] = 0; + for (int i = 0; i < num_parameters; ++i) { + ParameterBlock* block = + FindOrDie(parameter_map, const_cast<double*>(parameters[i])); + if (lift_covariance_to_ambient_space) { + parameter_sizes.push_back(block->Size()); + } else { + parameter_sizes.push_back(block->LocalSize()); + } + } + std::partial_sum(parameter_sizes.begin(), parameter_sizes.end(), + cum_parameter_size.begin() + 1); + const int max_covariance_block_size = + *std::max_element(parameter_sizes.begin(), parameter_sizes.end()); + const int covariance_size = cum_parameter_size.back(); + + // Assemble the blocks in the covariance matrix. + MatrixRef covariance(covariance_matrix, covariance_size, covariance_size); + const int num_threads = options_.num_threads; + scoped_array<double> workspace( + new double[num_threads * max_covariance_block_size * + max_covariance_block_size]); + + bool success = true; + +// The collapse() directive is only supported in OpenMP 3.0 and higher. OpenMP +// 3.0 was released in May 2008 (hence the version number). +#if _OPENMP >= 200805 +# pragma omp parallel for num_threads(num_threads) schedule(dynamic) collapse(2) +#else +# pragma omp parallel for num_threads(num_threads) schedule(dynamic) +#endif + for (int i = 0; i < num_parameters; ++i) { + for (int j = 0; j < num_parameters; ++j) { + // The second loop can't start from j = i for compatibility with OpenMP + // collapse command. The conditional serves as a workaround + if (j >= i) { + int covariance_row_idx = cum_parameter_size[i]; + int covariance_col_idx = cum_parameter_size[j]; + int size_i = parameter_sizes[i]; + int size_j = parameter_sizes[j]; +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + double* covariance_block = + workspace.get() + + thread_id * max_covariance_block_size * max_covariance_block_size; + if (!GetCovarianceBlockInTangentOrAmbientSpace( + parameters[i], parameters[j], lift_covariance_to_ambient_space, + covariance_block)) { + success = false; + } + + covariance.block(covariance_row_idx, covariance_col_idx, + size_i, size_j) = + MatrixRef(covariance_block, size_i, size_j); + + if (i != j) { + covariance.block(covariance_col_idx, covariance_row_idx, + size_j, size_i) = + MatrixRef(covariance_block, size_i, size_j).transpose(); + + } + } + } + } + return success; +} + // Determine the sparsity pattern of the covariance matrix based on // the block pairs requested by the user. bool CovarianceImpl::ComputeCovarianceSparsity( @@ -252,18 +397,28 @@ bool CovarianceImpl::ComputeCovarianceSparsity( vector<double*> all_parameter_blocks; problem->GetParameterBlocks(&all_parameter_blocks); const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map(); + HashSet<ParameterBlock*> parameter_blocks_in_use; + vector<ResidualBlock*> residual_blocks; + problem->GetResidualBlocks(&residual_blocks); + + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + parameter_blocks_in_use.insert(residual_block->parameter_blocks(), + residual_block->parameter_blocks() + + residual_block->NumParameterBlocks()); + } + 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 { + if (!block->IsConstant() && (parameter_blocks_in_use.count(block) > 0)) { active_parameter_blocks.push_back(parameter_block); + } else { + constant_parameter_blocks_.insert(parameter_block); } } @@ -386,8 +541,8 @@ bool CovarianceImpl::ComputeCovarianceValues() { switch (options_.algorithm_type) { case DENSE_SVD: return ComputeCovarianceValuesUsingDenseSVD(); -#ifndef CERES_NO_SUITESPARSE case SUITE_SPARSE_QR: +#ifndef CERES_NO_SUITESPARSE return ComputeCovarianceValuesUsingSuiteSparseQR(); #else LOG(ERROR) << "SuiteSparse is required to use the " @@ -624,7 +779,10 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() { if (automatic_truncation) { break; } else { - LOG(ERROR) << "Cholesky factorization of J'J is not reliable. " + LOG(ERROR) << "Error: Covariance matrix is near rank deficient " + << "and the user did not specify a non-zero" + << "Covariance::Options::null_space_rank " + << "to enable the computation of a Pseudo-Inverse. " << "Reciprocal condition number: " << singular_value_ratio * singular_value_ratio << " " << "min_reciprocal_condition_number: " diff --git a/extern/ceres/internal/ceres/covariance_impl.h b/extern/ceres/internal/ceres/covariance_impl.h index eb0cd040666..a3f0761f57c 100644 --- a/extern/ceres/internal/ceres/covariance_impl.h +++ b/extern/ceres/internal/ceres/covariance_impl.h @@ -55,12 +55,21 @@ class CovarianceImpl { const double*> >& covariance_blocks, ProblemImpl* problem); + bool Compute( + const std::vector<const double*>& parameter_blocks, + ProblemImpl* problem); + bool GetCovarianceBlockInTangentOrAmbientSpace( const double* parameter_block1, const double* parameter_block2, bool lift_covariance_to_ambient_space, double* covariance_block) const; + bool GetCovarianceMatrixInTangentOrAmbientSpace( + const std::vector<const double*>& parameters, + bool lift_covariance_to_ambient_space, + double *covariance_matrix) const; + bool ComputeCovarianceSparsity( const std::vector<std::pair<const double*, const double*> >& covariance_blocks, diff --git a/extern/ceres/internal/ceres/gradient_checker.cc b/extern/ceres/internal/ceres/gradient_checker.cc new file mode 100644 index 00000000000..c16c141db09 --- /dev/null +++ b/extern/ceres/internal/ceres/gradient_checker.cc @@ -0,0 +1,276 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 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. +// +// Authors: wjr@google.com (William Rucklidge), +// keir@google.com (Keir Mierle), +// dgossow@google.com (David Gossow) + +#include "ceres/gradient_checker.h" + +#include <algorithm> +#include <cmath> +#include <numeric> +#include <string> +#include <vector> + +#include "ceres/is_close.h" +#include "ceres/stringprintf.h" +#include "ceres/types.h" + +namespace ceres { + +using internal::IsClose; +using internal::StringAppendF; +using internal::StringPrintf; +using std::string; +using std::vector; + +namespace { +// Evaluate the cost function and transform the returned Jacobians to +// the local space of the respective local parameterizations. +bool EvaluateCostFunction( + const ceres::CostFunction* function, + double const* const * parameters, + const std::vector<const ceres::LocalParameterization*>& + local_parameterizations, + Vector* residuals, + std::vector<Matrix>* jacobians, + std::vector<Matrix>* local_jacobians) { + CHECK_NOTNULL(residuals); + CHECK_NOTNULL(jacobians); + CHECK_NOTNULL(local_jacobians); + + const vector<int32>& block_sizes = function->parameter_block_sizes(); + const int num_parameter_blocks = block_sizes.size(); + + // Allocate Jacobian matrices in local space. + local_jacobians->resize(num_parameter_blocks); + vector<double*> local_jacobian_data(num_parameter_blocks); + for (int i = 0; i < num_parameter_blocks; ++i) { + int block_size = block_sizes.at(i); + if (local_parameterizations.at(i) != NULL) { + block_size = local_parameterizations.at(i)->LocalSize(); + } + local_jacobians->at(i).resize(function->num_residuals(), block_size); + local_jacobians->at(i).setZero(); + local_jacobian_data.at(i) = local_jacobians->at(i).data(); + } + + // Allocate Jacobian matrices in global space. + jacobians->resize(num_parameter_blocks); + vector<double*> jacobian_data(num_parameter_blocks); + for (int i = 0; i < num_parameter_blocks; ++i) { + jacobians->at(i).resize(function->num_residuals(), block_sizes.at(i)); + jacobians->at(i).setZero(); + jacobian_data.at(i) = jacobians->at(i).data(); + } + + // Compute residuals & jacobians. + CHECK_NE(0, function->num_residuals()); + residuals->resize(function->num_residuals()); + residuals->setZero(); + if (!function->Evaluate(parameters, residuals->data(), + jacobian_data.data())) { + return false; + } + + // Convert Jacobians from global to local space. + for (size_t i = 0; i < local_jacobians->size(); ++i) { + if (local_parameterizations.at(i) == NULL) { + local_jacobians->at(i) = jacobians->at(i); + } else { + int global_size = local_parameterizations.at(i)->GlobalSize(); + int local_size = local_parameterizations.at(i)->LocalSize(); + CHECK_EQ(jacobians->at(i).cols(), global_size); + Matrix global_J_local(global_size, local_size); + local_parameterizations.at(i)->ComputeJacobian( + parameters[i], global_J_local.data()); + local_jacobians->at(i) = jacobians->at(i) * global_J_local; + } + } + return true; +} +} // namespace + +GradientChecker::GradientChecker( + const CostFunction* function, + const vector<const LocalParameterization*>* local_parameterizations, + const NumericDiffOptions& options) : + function_(function) { + CHECK_NOTNULL(function); + if (local_parameterizations != NULL) { + local_parameterizations_ = *local_parameterizations; + } else { + local_parameterizations_.resize(function->parameter_block_sizes().size(), + NULL); + } + DynamicNumericDiffCostFunction<CostFunction, CENTRAL>* + finite_diff_cost_function = + new DynamicNumericDiffCostFunction<CostFunction, CENTRAL>( + function, DO_NOT_TAKE_OWNERSHIP, options); + finite_diff_cost_function_.reset(finite_diff_cost_function); + + const vector<int32>& parameter_block_sizes = + function->parameter_block_sizes(); + const int num_parameter_blocks = parameter_block_sizes.size(); + for (int i = 0; i < num_parameter_blocks; ++i) { + finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]); + } + finite_diff_cost_function->SetNumResiduals(function->num_residuals()); +} + +bool GradientChecker::Probe(double const* const * parameters, + double relative_precision, + ProbeResults* results_param) const { + int num_residuals = function_->num_residuals(); + + // Make sure that we have a place to store results, no matter if the user has + // provided an output argument. + ProbeResults* results; + ProbeResults results_local; + if (results_param != NULL) { + results = results_param; + results->residuals.resize(0); + results->jacobians.clear(); + results->numeric_jacobians.clear(); + results->local_jacobians.clear(); + results->local_numeric_jacobians.clear(); + results->error_log.clear(); + } else { + results = &results_local; + } + results->maximum_relative_error = 0.0; + results->return_value = true; + + // Evaluate the derivative using the user supplied code. + vector<Matrix>& jacobians = results->jacobians; + vector<Matrix>& local_jacobians = results->local_jacobians; + if (!EvaluateCostFunction(function_, parameters, local_parameterizations_, + &results->residuals, &jacobians, &local_jacobians)) { + results->error_log = "Function evaluation with Jacobians failed."; + results->return_value = false; + } + + // Evaluate the derivative using numeric derivatives. + vector<Matrix>& numeric_jacobians = results->numeric_jacobians; + vector<Matrix>& local_numeric_jacobians = results->local_numeric_jacobians; + Vector finite_diff_residuals; + if (!EvaluateCostFunction(finite_diff_cost_function_.get(), parameters, + local_parameterizations_, &finite_diff_residuals, + &numeric_jacobians, &local_numeric_jacobians)) { + results->error_log += "\nFunction evaluation with numerical " + "differentiation failed."; + results->return_value = false; + } + + if (!results->return_value) { + return false; + } + + for (int i = 0; i < num_residuals; ++i) { + if (!IsClose( + results->residuals[i], + finite_diff_residuals[i], + relative_precision, + NULL, + NULL)) { + results->error_log = "Function evaluation with and without Jacobians " + "resulted in different residuals."; + LOG(INFO) << results->residuals.transpose(); + LOG(INFO) << finite_diff_residuals.transpose(); + return false; + } + } + + // See if any elements have relative error larger than the threshold. + int num_bad_jacobian_components = 0; + double& worst_relative_error = results->maximum_relative_error; + 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 error_log; + for (int k = 0; k < function_->parameter_block_sizes().size(); k++) { + StringAppendF(&error_log, + "========== " + "Jacobian for " "block %d: (%ld by %ld)) " + "==========\n", + k, + static_cast<long>(local_jacobians[k].rows()), + static_cast<long>(local_jacobians[k].cols())); + // The funny spacing creates appropriately aligned column headers. + error_log += + " block row col user dx/dy num diff dx/dy " + "abs error relative error parameter residual\n"; + + for (int i = 0; i < local_jacobians[k].rows(); i++) { + for (int j = 0; j < local_jacobians[k].cols(); j++) { + double term_jacobian = local_jacobians[k](i, j); + double finite_jacobian = local_numeric_jacobians[k](i, j); + double relative_error, absolute_error; + bool bad_jacobian_entry = + !IsClose(term_jacobian, + finite_jacobian, + relative_precision, + &relative_error, + &absolute_error); + worst_relative_error = std::max(worst_relative_error, relative_error); + + StringAppendF(&error_log, + "%6d %4d %4d %17g %17g %17g %17g %17g %17g", + k, i, j, + term_jacobian, finite_jacobian, + absolute_error, relative_error, + parameters[k][j], + results->residuals[i]); + + if (bad_jacobian_entry) { + num_bad_jacobian_components++; + StringAppendF( + &error_log, + " ------ (%d,%d,%d) Relative error worse than %g", + k, i, j, relative_precision); + } + error_log += "\n"; + } + } + } + + // Since there were some bad errors, dump comprehensive debug info. + if (num_bad_jacobian_components) { + string header = StringPrintf("\nDetected %d bad Jacobian component(s). " + "Worst relative error was %g.\n", + num_bad_jacobian_components, + worst_relative_error); + results->error_log = header + "\n" + error_log; + return false; + } + return true; +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/gradient_checking_cost_function.cc b/extern/ceres/internal/ceres/gradient_checking_cost_function.cc index 580fd260e15..f2c73367891 100644 --- a/extern/ceres/internal/ceres/gradient_checking_cost_function.cc +++ b/extern/ceres/internal/ceres/gradient_checking_cost_function.cc @@ -26,7 +26,8 @@ // 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) +// Authors: keir@google.com (Keir Mierle), +// dgossow@google.com (David Gossow) #include "ceres/gradient_checking_cost_function.h" @@ -36,7 +37,7 @@ #include <string> #include <vector> -#include "ceres/cost_function.h" +#include "ceres/gradient_checker.h" #include "ceres/internal/eigen.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/parameter_block.h" @@ -59,55 +60,25 @@ 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) + GradientCheckingCostFunction( + const CostFunction* function, + const std::vector<const LocalParameterization*>* local_parameterizations, + const NumericDiffOptions& options, + double relative_precision, + const string& extra_info, + GradientCheckingIterationCallback* callback) : function_(function), + gradient_checker_(function, local_parameterizations, options), 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); - + extra_info_(extra_info), + callback_(callback) { + CHECK_NOTNULL(callback_); 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() { } @@ -120,133 +91,92 @@ class GradientCheckingCostFunction : public CostFunction { return function_->Evaluate(parameters, residuals, NULL); } - int num_residuals = function_->num_residuals(); + GradientChecker::ProbeResults results; + bool okay = gradient_checker_.Probe(parameters, + relative_precision_, + &results); - // 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."; + // If the cost function returned false, there's nothing we can say about + // the gradients. + if (results.return_value == false) { return false; } - // Evaluate the derivative using numeric derivatives. - finite_diff_cost_function_->Evaluate( - parameters, - residuals, - &finite_difference_jacobian_pointers[0]); + // Copy the residuals. + const int num_residuals = function_->num_residuals(); + MatrixRef(residuals, num_residuals, 1) = results.residuals; - // 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; + // Copy the original jacobian blocks into the jacobians array. + const vector<int32>& block_sizes = function_->parameter_block_sizes(); 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"; - } + results.jacobians[k].rows(), + results.jacobians[k].cols()) = results.jacobians[k]; } } - // 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; + if (!okay) { + std::string error_log = "Gradient Error detected!\nExtra info for " + "this residual: " + extra_info_ + "\n" + results.error_log; + callback_->SetGradientErrorDetected(error_log); } return true; } private: const CostFunction* function_; - internal::scoped_ptr<CostFunction> finite_diff_cost_function_; + GradientChecker gradient_checker_; double relative_precision_; string extra_info_; + GradientCheckingIterationCallback* callback_; }; } // namespace -CostFunction *CreateGradientCheckingCostFunction( - const CostFunction *cost_function, +GradientCheckingIterationCallback::GradientCheckingIterationCallback() + : gradient_error_detected_(false) { +} + +CallbackReturnType GradientCheckingIterationCallback::operator()( + const IterationSummary& summary) { + if (gradient_error_detected_) { + LOG(ERROR)<< "Gradient error detected. Terminating solver."; + return SOLVER_ABORT; + } + return SOLVER_CONTINUE; +} +void GradientCheckingIterationCallback::SetGradientErrorDetected( + std::string& error_log) { + mutex_.Lock(); + gradient_error_detected_ = true; + error_log_ += "\n" + error_log; + mutex_.Unlock(); +} + +CostFunction* CreateGradientCheckingCostFunction( + const CostFunction* cost_function, + const std::vector<const LocalParameterization*>* local_parameterizations, double relative_step_size, double relative_precision, - const string& extra_info) { + const std::string& extra_info, + GradientCheckingIterationCallback* callback) { NumericDiffOptions numeric_diff_options; numeric_diff_options.relative_step_size = relative_step_size; return new GradientCheckingCostFunction(cost_function, + local_parameterizations, numeric_diff_options, - relative_precision, - extra_info); + relative_precision, extra_info, + callback); } -ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, - double relative_step_size, - double relative_precision) { +ProblemImpl* CreateGradientCheckingProblemImpl( + ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision, + GradientCheckingIterationCallback* callback) { + CHECK_NOTNULL(callback); // 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 @@ -260,6 +190,9 @@ ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, gradient_checking_problem_options.local_parameterization_ownership = DO_NOT_TAKE_OWNERSHIP; + NumericDiffOptions numeric_diff_options; + numeric_diff_options.relative_step_size = relative_step_size; + ProblemImpl* gradient_checking_problem_impl = new ProblemImpl( gradient_checking_problem_options); @@ -294,19 +227,26 @@ ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, string extra_info = StringPrintf( "Residual block id %d; depends on parameters [", i); vector<double*> parameter_blocks; + vector<const LocalParameterization*> local_parameterizations; + parameter_blocks.reserve(residual_block->NumParameterBlocks()); + local_parameterizations.reserve(residual_block->NumParameterBlocks()); 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) ? ", " : "]"; + local_parameterizations.push_back(problem_impl->GetParameterization( + parameter_block->mutable_user_state())); } // Wrap the original CostFunction in a GradientCheckingCostFunction. CostFunction* gradient_checking_cost_function = - CreateGradientCheckingCostFunction(residual_block->cost_function(), - relative_step_size, - relative_precision, - extra_info); + new GradientCheckingCostFunction(residual_block->cost_function(), + &local_parameterizations, + numeric_diff_options, + relative_precision, + extra_info, + callback); // The const_cast is necessary because // ProblemImpl::AddResidualBlock can potentially take ownership of diff --git a/extern/ceres/internal/ceres/gradient_checking_cost_function.h b/extern/ceres/internal/ceres/gradient_checking_cost_function.h index cf92cb72bc5..497f8e2a594 100644 --- a/extern/ceres/internal/ceres/gradient_checking_cost_function.h +++ b/extern/ceres/internal/ceres/gradient_checking_cost_function.h @@ -26,7 +26,8 @@ // 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) +// Authors: keir@google.com (Keir Mierle), +// dgossow@google.com (David Gossow) #ifndef CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ #define CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ @@ -34,50 +35,76 @@ #include <string> #include "ceres/cost_function.h" +#include "ceres/iteration_callback.h" +#include "ceres/local_parameterization.h" +#include "ceres/mutex.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. +// Callback that collects information about gradient checking errors, and +// will abort the solve as soon as an error occurs. +class GradientCheckingIterationCallback : public IterationCallback { + public: + GradientCheckingIterationCallback(); + + // Will return SOLVER_CONTINUE until a gradient error has been detected, + // then return SOLVER_ABORT. + virtual CallbackReturnType operator()(const IterationSummary& summary); + + // Notify this that a gradient error has occurred (thread safe). + void SetGradientErrorDetected(std::string& error_log); + + // Retrieve error status (not thread safe). + bool gradient_error_detected() const { return gradient_error_detected_; } + const std::string& error_log() const { return error_log_; } + private: + bool gradient_error_detected_; + std::string error_log_; + // Mutex protecting member variables. + ceres::internal::Mutex mutex_; +}; + +// Creates a CostFunction that checks the Jacobians that cost_function computes +// with finite differences. This API is only intended for unit tests that intend +// to check the functionality of the GradientCheckingCostFunction +// implementation directly. CostFunction* CreateGradientCheckingCostFunction( const CostFunction* cost_function, + const std::vector<const LocalParameterization*>* local_parameterizations, double relative_step_size, double relative_precision, - const std::string& extra_info); + const std::string& extra_info, + GradientCheckingIterationCallback* callback); -// 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. +// Create a new ProblemImpl object from the input problem_impl, where all +// cost functions are wrapped so that each time their Evaluate method is called, +// an additional check is performed that compares the Jacobians computed by +// the original cost function with alternative Jacobians computed using +// numerical differentiation. If local parameterizations are given for any +// parameters, the Jacobians will be compared in the local space instead of the +// ambient space. For details on the gradient checking procedure, see the +// documentation of the GradientChecker class. If an error is detected in any +// iteration, the respective cost function will notify the +// GradientCheckingIterationCallback. +// +// The caller owns the returned ProblemImpl object. +// +// Note: This is quite inefficient and is intended only for debugging. // // 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); +// jacobians obtained by numerically differentiating them. See the +// documentation of 'numeric_derivative_relative_step_size' in solver.h for a +// better explanation. +ProblemImpl* CreateGradientCheckingProblemImpl( + ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision, + GradientCheckingIterationCallback* callback); } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/gradient_problem_solver.cc b/extern/ceres/internal/ceres/gradient_problem_solver.cc index 9a549c23dac..8709f8f3fbd 100644 --- a/extern/ceres/internal/ceres/gradient_problem_solver.cc +++ b/extern/ceres/internal/ceres/gradient_problem_solver.cc @@ -84,6 +84,12 @@ Solver::Options GradientProblemSolverOptionsToSolverOptions( } // namespace +bool GradientProblemSolver::Options::IsValid(std::string* error) const { + const Solver::Options solver_options = + GradientProblemSolverOptionsToSolverOptions(*this); + return solver_options.IsValid(error); +} + GradientProblemSolver::~GradientProblemSolver() { } @@ -99,8 +105,6 @@ void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options, using internal::SetSummaryFinalCost; double start_time = WallTimeInSeconds(); - Solver::Options solver_options = - GradientProblemSolverOptionsToSolverOptions(options); *CHECK_NOTNULL(summary) = Summary(); summary->num_parameters = problem.NumParameters(); @@ -112,14 +116,16 @@ void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options, summary->nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; // NOLINT // Check validity - if (!solver_options.IsValid(&summary->message)) { + if (!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); + // TODO(sameeragarwal): This is a bit convoluted, we should be able + // to convert to minimizer options directly, but this will do for + // now. + Minimizer::Options minimizer_options = + Minimizer::Options(GradientProblemSolverOptionsToSolverOptions(options)); minimizer_options.evaluator.reset(new GradientProblemEvaluator(problem)); scoped_ptr<IterationCallback> logging_callback; diff --git a/extern/ceres/internal/ceres/is_close.cc b/extern/ceres/internal/ceres/is_close.cc new file mode 100644 index 00000000000..a91a17454d9 --- /dev/null +++ b/extern/ceres/internal/ceres/is_close.cc @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 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. +// +// Authors: keir@google.com (Keir Mierle), dgossow@google.com (David Gossow) + +#include "ceres/is_close.h" + +#include <algorithm> +#include <cmath> + +namespace ceres { +namespace internal { +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 = std::fabs(x - y); + *relative_error = *absolute_error / std::max(std::fabs(x), std::fabs(y)); + if (x == 0 || y == 0) { + // If x or y is exactly zero, then relative difference doesn't have any + // meaning. Take the absolute difference instead. + *relative_error = *absolute_error; + } + return *relative_error < std::fabs(relative_precision); +} +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/is_close.h b/extern/ceres/internal/ceres/is_close.h new file mode 100644 index 00000000000..7789448c8e8 --- /dev/null +++ b/extern/ceres/internal/ceres/is_close.h @@ -0,0 +1,51 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 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. +// +// Authors: keir@google.com (Keir Mierle), dgossow@google.com (David Gossow) +// +// Utility routine for comparing two values. + +#ifndef CERES_INTERNAL_IS_CLOSE_H_ +#define CERES_INTERNAL_IS_CLOSE_H_ + +namespace ceres { +namespace internal { +// Returns true if x and y have a relative (unsigned) difference less than +// relative_precision and false otherwise. Stores the relative and absolute +// difference in relative/absolute_error if non-NULL. If one of the two values +// is exactly zero, the absolute difference will be compared, and relative_error +// will be set to the absolute difference. +bool IsClose(double x, + double y, + double relative_precision, + double *relative_error, + double *absolute_error); +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_IS_CLOSE_H_ diff --git a/extern/ceres/internal/ceres/line_search_minimizer.cc b/extern/ceres/internal/ceres/line_search_minimizer.cc index 62264fb0b64..fdde1ca9c86 100644 --- a/extern/ceres/internal/ceres/line_search_minimizer.cc +++ b/extern/ceres/internal/ceres/line_search_minimizer.cc @@ -191,6 +191,7 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, options.line_search_sufficient_curvature_decrease; line_search_options.max_step_expansion = options.max_line_search_step_expansion; + line_search_options.is_silent = options.is_silent; line_search_options.function = &line_search_function; scoped_ptr<LineSearch> @@ -341,10 +342,12 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, "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)) { + } + + 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 " @@ -352,15 +355,17 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, summary->message; LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; break; - } else { - x = x_plus_delta; } + // Compute the norm of the step in the ambient space. + iteration_summary.step_norm = (x_plus_delta - x).norm(); + 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; @@ -376,6 +381,13 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, WallTimeInSeconds() - start_time + summary->preprocessor_time_in_seconds; + // Iterations inside the line search algorithm are considered + // 'steps' in the broader context, to distinguish these inner + // iterations from from the outer iterations of the line search + // minimizer. The number of line search steps is the total number + // of inner line search iterations (or steps) across the entire + // minimization. + summary->num_line_search_steps += line_search_summary.num_iterations; summary->line_search_cost_evaluation_time_in_seconds += line_search_summary.cost_evaluation_time_in_seconds; summary->line_search_gradient_evaluation_time_in_seconds += diff --git a/extern/ceres/internal/ceres/local_parameterization.cc b/extern/ceres/internal/ceres/local_parameterization.cc index 82004761ec0..a6bf1f6ddcc 100644 --- a/extern/ceres/internal/ceres/local_parameterization.cc +++ b/extern/ceres/internal/ceres/local_parameterization.cc @@ -30,6 +30,8 @@ #include "ceres/local_parameterization.h" +#include <algorithm> +#include "Eigen/Geometry" #include "ceres/householder_vector.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" @@ -87,28 +89,17 @@ bool IdentityParameterization::MultiplyByJacobian(const double* x, } 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"; - + int size, const vector<int>& constant_parameters) + : local_size_(size - constant_parameters.size()), constancy_mask_(size, 0) { vector<int> constant = constant_parameters; - sort(constant.begin(), constant.end()); - CHECK(unique(constant.begin(), constant.end()) == constant.end()) + std::sort(constant.begin(), constant.end()); + CHECK_GE(constant.front(), 0) + << "Indices indicating constant parameter must be greater than zero."; + CHECK_LT(constant.back(), size) + << "Indices indicating constant parameter must be less than the size " + << "of the parameter block."; + CHECK(std::adjacent_find(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; } @@ -129,6 +120,10 @@ bool SubsetParameterization::Plus(const double* x, bool SubsetParameterization::ComputeJacobian(const double* x, double* jacobian) const { + if (local_size_ == 0) { + return true; + } + MatrixRef m(jacobian, constancy_mask_.size(), local_size_); m.setZero(); for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { @@ -143,6 +138,10 @@ bool SubsetParameterization::MultiplyByJacobian(const double* x, const int num_rows, const double* global_matrix, double* local_matrix) const { + if (local_size_ == 0) { + return true; + } + for (int row = 0; row < num_rows; ++row) { for (int col = 0, j = 0; col < constancy_mask_.size(); ++col) { if (!constancy_mask_[col]) { @@ -184,6 +183,39 @@ bool QuaternionParameterization::ComputeJacobian(const double* x, return true; } +bool EigenQuaternionParameterization::Plus(const double* x_ptr, + const double* delta, + double* x_plus_delta_ptr) const { + Eigen::Map<Eigen::Quaterniond> x_plus_delta(x_plus_delta_ptr); + Eigen::Map<const Eigen::Quaterniond> x(x_ptr); + + 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; + + // Note, in the constructor w is first. + Eigen::Quaterniond delta_q(cos(norm_delta), + sin_delta_by_delta * delta[0], + sin_delta_by_delta * delta[1], + sin_delta_by_delta * delta[2]); + x_plus_delta = delta_q * x; + } else { + x_plus_delta = x; + } + + return true; +} + +bool EigenQuaternionParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT + jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT + jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT + jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT + return true; +} + HomogeneousVectorParameterization::HomogeneousVectorParameterization(int size) : size_(size) { CHECK_GT(size_, 1) << "The size of the homogeneous vector needs to be " @@ -332,9 +364,9 @@ bool ProductParameterization::ComputeJacobian(const double* x, 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; } diff --git a/extern/ceres/internal/ceres/map_util.h b/extern/ceres/internal/ceres/map_util.h index 61c531f297c..f55aee37689 100644 --- a/extern/ceres/internal/ceres/map_util.h +++ b/extern/ceres/internal/ceres/map_util.h @@ -67,7 +67,7 @@ FindOrDie(const Collection& collection, // 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& +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) { diff --git a/extern/ceres/internal/ceres/parameter_block.h b/extern/ceres/internal/ceres/parameter_block.h index cb7140d9582..8e21553c668 100644 --- a/extern/ceres/internal/ceres/parameter_block.h +++ b/extern/ceres/internal/ceres/parameter_block.h @@ -161,25 +161,34 @@ class ParameterBlock { // does not take ownership of the parameterization. void SetParameterization(LocalParameterization* new_parameterization) { CHECK(new_parameterization != NULL) << "NULL parameterization invalid."; + // Nothing to do if the new parameterization is the same as the + // old parameterization. + if (new_parameterization == local_parameterization_) { + return; + } + + CHECK(local_parameterization_ == NULL) + << "Can't re-set the local parameterization; it leads to " + << "ambiguous ownership. Current local parameterization is: " + << local_parameterization_; + 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. - } + + CHECK_GT(new_parameterization->LocalSize(), 0) + << "Invalid parameterization. Parameterizations must have a positive " + << "dimensional tangent space."; + + 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(); } void SetUpperBound(int index, double upper_bound) { diff --git a/extern/ceres/internal/ceres/problem.cc b/extern/ceres/internal/ceres/problem.cc index 03b7d6afa48..730ce642036 100644 --- a/extern/ceres/internal/ceres/problem.cc +++ b/extern/ceres/internal/ceres/problem.cc @@ -174,6 +174,10 @@ void Problem::SetParameterBlockVariable(double* values) { problem_impl_->SetParameterBlockVariable(values); } +bool Problem::IsParameterBlockConstant(double* values) const { + return problem_impl_->IsParameterBlockConstant(values); +} + void Problem::SetParameterization( double* values, LocalParameterization* local_parameterization) { diff --git a/extern/ceres/internal/ceres/problem_impl.cc b/extern/ceres/internal/ceres/problem_impl.cc index 8547d5d3f77..4abea8b33ee 100644 --- a/extern/ceres/internal/ceres/problem_impl.cc +++ b/extern/ceres/internal/ceres/problem_impl.cc @@ -249,10 +249,11 @@ ResidualBlock* ProblemImpl::AddResidualBlock( // 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()) { + const bool has_duplicate_items = + (std::adjacent_find(sorted_parameter_blocks.begin(), + sorted_parameter_blocks.end()) + != sorted_parameter_blocks.end()); + if (has_duplicate_items) { string blocks; for (int i = 0; i < parameter_blocks.size(); ++i) { blocks += StringPrintf(" %p ", parameter_blocks[i]); @@ -572,6 +573,16 @@ void ProblemImpl::SetParameterBlockConstant(double* values) { parameter_block->SetConstant(); } +bool ProblemImpl::IsParameterBlockConstant(double* values) const { + const ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, values, NULL); + CHECK(parameter_block != NULL) + << "Parameter block not found: " << values << ". You must add the " + << "parameter block to the problem before it can be queried."; + + return parameter_block->IsConstant(); +} + void ProblemImpl::SetParameterBlockVariable(double* values) { ParameterBlock* parameter_block = FindWithDefault(parameter_block_map_, values, NULL); diff --git a/extern/ceres/internal/ceres/problem_impl.h b/extern/ceres/internal/ceres/problem_impl.h index f42bde6c793..a4689c362f6 100644 --- a/extern/ceres/internal/ceres/problem_impl.h +++ b/extern/ceres/internal/ceres/problem_impl.h @@ -128,6 +128,8 @@ class ProblemImpl { void SetParameterBlockConstant(double* values); void SetParameterBlockVariable(double* values); + bool IsParameterBlockConstant(double* values) const; + void SetParameterization(double* values, LocalParameterization* local_parameterization); const LocalParameterization* GetParameterization(double* values) const; diff --git a/extern/ceres/internal/ceres/reorder_program.cc b/extern/ceres/internal/ceres/reorder_program.cc index d0e8f32b3b7..a7c37107591 100644 --- a/extern/ceres/internal/ceres/reorder_program.cc +++ b/extern/ceres/internal/ceres/reorder_program.cc @@ -142,6 +142,11 @@ void OrderingForSparseNormalCholeskyUsingSuiteSparse( ordering); } + VLOG(2) << "Block ordering stats: " + << " flops: " << ss.mutable_cc()->fl + << " lnz : " << ss.mutable_cc()->lnz + << " anz : " << ss.mutable_cc()->anz; + ss.Free(block_jacobian_transpose); #endif // CERES_NO_SUITESPARSE } diff --git a/extern/ceres/internal/ceres/residual_block.h b/extern/ceres/internal/ceres/residual_block.h index 05e6d1f81e5..a32f1c36cd3 100644 --- a/extern/ceres/internal/ceres/residual_block.h +++ b/extern/ceres/internal/ceres/residual_block.h @@ -127,7 +127,7 @@ class ResidualBlock { int index() const { return index_; } void set_index(int index) { index_ = index; } - std::string ToString() { + std::string ToString() const { return StringPrintf("{residual block; index=%d}", index_); } diff --git a/extern/ceres/internal/ceres/schur_complement_solver.cc b/extern/ceres/internal/ceres/schur_complement_solver.cc index 2491060dcdc..65449832c4c 100644 --- a/extern/ceres/internal/ceres/schur_complement_solver.cc +++ b/extern/ceres/internal/ceres/schur_complement_solver.cc @@ -33,6 +33,7 @@ #include <algorithm> #include <ctime> #include <set> +#include <sstream> #include <vector> #include "ceres/block_random_access_dense_matrix.h" @@ -563,6 +564,12 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingEigen( // worse than the one computed using the block version of the // algorithm. simplicial_ldlt_->analyzePattern(eigen_lhs); + if (VLOG_IS_ON(2)) { + std::stringstream ss; + simplicial_ldlt_->dumpMemory(ss); + VLOG(2) << "Symbolic Analysis\n" + << ss.str(); + } event_logger.AddEvent("Analysis"); if (simplicial_ldlt_->info() != Eigen::Success) { summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; diff --git a/extern/ceres/internal/ceres/solver.cc b/extern/ceres/internal/ceres/solver.cc index 9f3228bb0be..8411350986a 100644 --- a/extern/ceres/internal/ceres/solver.cc +++ b/extern/ceres/internal/ceres/solver.cc @@ -94,7 +94,7 @@ bool CommonOptionsAreValid(const Solver::Options& options, string* error) { 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); + OPTION_GT(gradient_check_numeric_derivative_relative_step_size, 0.0); } return true; } @@ -351,6 +351,7 @@ void PreSolveSummarize(const Solver::Options& options, 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->num_line_search_steps = 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; @@ -495,21 +496,28 @@ void Solver::Solve(const Solver::Options& options, // values provided by the user. program->SetParameterBlockStatePtrsToUserStatePtrs(); + // If gradient_checking is enabled, wrap all cost functions in a + // gradient checker and install a callback that terminates if any gradient + // error is detected. scoped_ptr<internal::ProblemImpl> gradient_checking_problem; + internal::GradientCheckingIterationCallback gradient_checking_callback; + Solver::Options modified_options = options; if (options.check_gradients) { + modified_options.callbacks.push_back(&gradient_checking_callback); gradient_checking_problem.reset( CreateGradientCheckingProblemImpl( problem_impl, - options.numeric_derivative_relative_step_size, - options.gradient_check_relative_precision)); + options.gradient_check_numeric_derivative_relative_step_size, + options.gradient_check_relative_precision, + &gradient_checking_callback)); problem_impl = gradient_checking_problem.get(); program = problem_impl->mutable_program(); } scoped_ptr<Preprocessor> preprocessor( - Preprocessor::Create(options.minimizer_type)); + Preprocessor::Create(modified_options.minimizer_type)); PreprocessedProblem pp; - const bool status = preprocessor->Preprocess(options, problem_impl, &pp); + const bool status = preprocessor->Preprocess(modified_options, problem_impl, &pp); summary->fixed_cost = pp.fixed_cost; summary->preprocessor_time_in_seconds = WallTimeInSeconds() - start_time; @@ -534,6 +542,13 @@ void Solver::Solve(const Solver::Options& options, summary->postprocessor_time_in_seconds = WallTimeInSeconds() - postprocessor_start_time; + // If the gradient checker reported an error, we want to report FAILURE + // instead of USER_FAILURE and provide the error log. + if (gradient_checking_callback.gradient_error_detected()) { + summary->termination_type = FAILURE; + summary->message = gradient_checking_callback.error_log(); + } + summary->total_time_in_seconds = WallTimeInSeconds() - start_time; } @@ -556,6 +571,7 @@ Solver::Summary::Summary() num_successful_steps(-1), num_unsuccessful_steps(-1), num_inner_iteration_steps(-1), + num_line_search_steps(-1), preprocessor_time_in_seconds(-1.0), minimizer_time_in_seconds(-1.0), postprocessor_time_in_seconds(-1.0), @@ -696,16 +712,14 @@ string Solver::Summary::FullReport() const { 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()); - } + 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, @@ -784,9 +798,14 @@ string Solver::Summary::FullReport() const { num_inner_iteration_steps); } - const bool print_line_search_timing_information = - minimizer_type == LINE_SEARCH || - (minimizer_type == TRUST_REGION && is_constrained); + const bool line_search_used = + (minimizer_type == LINE_SEARCH || + (minimizer_type == TRUST_REGION && is_constrained)); + + if (line_search_used) { + StringAppendF(&report, "Line search steps % 14d\n", + num_line_search_steps); + } StringAppendF(&report, "\nTime (in seconds):\n"); StringAppendF(&report, "Preprocessor %25.4f\n", @@ -794,13 +813,13 @@ string Solver::Summary::FullReport() const { StringAppendF(&report, "\n Residual evaluation %23.4f\n", residual_evaluation_time_in_seconds); - if (print_line_search_timing_information) { + if (line_search_used) { 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) { + if (line_search_used) { StringAppendF(&report, " Line search gradient evaluation %6.4f\n", line_search_gradient_evaluation_time_in_seconds); } @@ -815,7 +834,7 @@ string Solver::Summary::FullReport() const { inner_iteration_time_in_seconds); } - if (print_line_search_timing_information) { + if (line_search_used) { StringAppendF(&report, " Line search polynomial minimization %.4f\n", line_search_polynomial_minimization_time_in_seconds); } diff --git a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc index ed00879b47a..a4c2c766ddc 100644 --- a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc +++ b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc @@ -33,6 +33,7 @@ #include <algorithm> #include <cstring> #include <ctime> +#include <sstream> #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/cxsparse.h" @@ -71,6 +72,12 @@ LinearSolver::Summary SimplicialLDLTSolve( if (do_symbolic_analysis) { solver->analyzePattern(lhs); + if (VLOG_IS_ON(2)) { + std::stringstream ss; + solver->dumpMemory(ss); + VLOG(2) << "Symbolic Analysis\n" + << ss.str(); + } event_logger->AddEvent("Analyze"); if (solver->info() != Eigen::Success) { summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; diff --git a/extern/ceres/internal/ceres/stringprintf.cc b/extern/ceres/internal/ceres/stringprintf.cc index d1d8b5fe8ab..b3b7474d8f8 100644 --- a/extern/ceres/internal/ceres/stringprintf.cc +++ b/extern/ceres/internal/ceres/stringprintf.cc @@ -43,14 +43,27 @@ 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 +// va_copy() was defined in the C99 standard. However, it did not appear in the +// C++ standard until C++11. This means that if Ceres is being compiled with a +// strict pre-C++11 standard (e.g. -std=c++03), va_copy() will NOT be defined, +// as we are using the C++ compiler (it would however be defined if we were +// using the C compiler). Note however that both GCC & Clang will in fact +// define va_copy() when compiling for C++ if the C++ standard is not explicitly +// specified (i.e. no -std=c++<XX> arg), even though it should not strictly be +// defined unless -std=c++11 (or greater) was passed. +#if !defined(va_copy) +#if defined (__GNUC__) +// On GCC/Clang, if va_copy() is not defined (C++ standard < C++11 explicitly +// specified), use the internal __va_copy() version, which should be present +// in even very old GCC versions. +#define va_copy(d, s) __va_copy(d, s) #else -enum { IS_COMPILER_MSVC = 0 }; -#endif +// Some older versions of MSVC do not have va_copy(), in which case define it. +// Although this is required for older MSVC versions, it should also work for +// other non-GCC/Clang compilers which also do not defined va_copy(). +#define va_copy(d, s) ((d) = (s)) +#endif // defined (__GNUC__) +#endif // !defined(va_copy) void StringAppendV(string* dst, const char* format, va_list ap) { // First try with a small fixed size buffer @@ -71,13 +84,13 @@ void StringAppendV(string* dst, const char* format, va_list ap) { 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 defined (_MSC_VER) + // 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); +#endif if (result < 0) { // Just an error. diff --git a/extern/ceres/internal/ceres/trust_region_minimizer.cc b/extern/ceres/internal/ceres/trust_region_minimizer.cc index d654d0867f1..d809906ab54 100644 --- a/extern/ceres/internal/ceres/trust_region_minimizer.cc +++ b/extern/ceres/internal/ceres/trust_region_minimizer.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2016 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -43,674 +43,747 @@ #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" +// Helper macro to simplify some of the control flow. +#define RETURN_IF_ERROR_AND_LOG(expr) \ + do { \ + if (!(expr)) { \ + LOG(ERROR) << "Terminating: " << solver_summary_->message; \ + return; \ + } \ + } while (0) + 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); +TrustRegionMinimizer::~TrustRegionMinimizer() {} - 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; +void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* solver_summary) { + start_time_in_secs_ = WallTimeInSeconds(); + iteration_start_time_in_secs_ = start_time_in_secs_; + Init(options, parameters, solver_summary); + RETURN_IF_ERROR_AND_LOG(IterationZero()); + + // Create the TrustRegionStepEvaluator. The construction needs to be + // delayed to this point because we need the cost for the starting + // point to initialize the step evaluator. + step_evaluator_.reset(new TrustRegionStepEvaluator( + x_cost_, + options_.use_nonmonotonic_steps + ? options_.max_consecutive_nonmonotonic_steps + : 0)); + + while (FinalizeIterationAndCheckIfMinimizerCanContinue()) { + iteration_start_time_in_secs_ = WallTimeInSeconds(); + iteration_summary_ = IterationSummary(); + iteration_summary_.iteration = + solver_summary->iterations.back().iteration + 1; + + RETURN_IF_ERROR_AND_LOG(ComputeTrustRegionStep()); + if (!iteration_summary_.step_is_valid) { + RETURN_IF_ERROR_AND_LOG(HandleInvalidStep()); + continue; + } - 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; -} + if (options_.is_constrained) { + // Use a projected line search to enforce the bounds constraints + // and improve the quality of the step. + DoLineSearch(x_, gradient_, x_cost_, &delta_); + } + + ComputeCandidatePointAndEvaluateCost(); + DoInnerIterationsIfNeeded(); -} // namespace + if (ParameterToleranceReached()) { + return; + } + + if (FunctionToleranceReached()) { + return; + } -// 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])); + if (IsStepSuccessful()) { + RETURN_IF_ERROR_AND_LOG(HandleSuccessfulStep()); + continue; + } + + HandleUnsuccessfulStep(); } } -void TrustRegionMinimizer::Init(const Minimizer::Options& options) { +// Initialize the minimizer, allocate working space and set some of +// the fields in the solver_summary. +void TrustRegionMinimizer::Init(const Minimizer::Options& options, + double* parameters, + Solver::Summary* solver_summary) { options_ = options; sort(options_.trust_region_minimizer_iterations_to_dump.begin(), options_.trust_region_minimizer_iterations_to_dump.end()); + + parameters_ = parameters; + + solver_summary_ = solver_summary; + solver_summary_->termination_type = NO_CONVERGENCE; + solver_summary_->num_successful_steps = 0; + solver_summary_->num_unsuccessful_steps = 0; + solver_summary_->is_constrained = options.is_constrained; + + evaluator_ = CHECK_NOTNULL(options_.evaluator.get()); + jacobian_ = CHECK_NOTNULL(options_.jacobian.get()); + strategy_ = CHECK_NOTNULL(options_.trust_region_strategy.get()); + + is_not_silent_ = !options.is_silent; + inner_iterations_are_enabled_ = + options.inner_iteration_minimizer.get() != NULL; + inner_iterations_were_useful_ = false; + + num_parameters_ = evaluator_->NumParameters(); + num_effective_parameters_ = evaluator_->NumEffectiveParameters(); + num_residuals_ = evaluator_->NumResiduals(); + num_consecutive_invalid_steps_ = 0; + + x_ = ConstVectorRef(parameters_, num_parameters_); + x_norm_ = x_.norm(); + residuals_.resize(num_residuals_); + trust_region_step_.resize(num_effective_parameters_); + delta_.resize(num_effective_parameters_); + candidate_x_.resize(num_parameters_); + gradient_.resize(num_effective_parameters_); + model_residuals_.resize(num_residuals_); + negative_gradient_.resize(num_effective_parameters_); + projected_gradient_step_.resize(num_parameters_); + + // By default scaling is one, if the user requests Jacobi scaling of + // the Jacobian, we will compute and overwrite this vector. + jacobian_scaling_ = Vector::Ones(num_effective_parameters_); + + x_norm_ = -1; // Invalid value + x_cost_ = std::numeric_limits<double>::max(); + minimum_cost_ = x_cost_; + model_cost_change_ = 0.0; } -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 = +// 1. Project the initial solution onto the feasible set if needed. +// 2. Compute the initial cost, jacobian & gradient. +// +// Return true if all computations can be performed successfully. +bool TrustRegionMinimizer::IterationZero() { + iteration_summary_ = IterationSummary(); + 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_.eta = options_.eta; + iteration_summary_.linear_solver_iterations = 0; + iteration_summary_.step_solver_time_in_seconds = 0; + + if (options_.is_constrained) { + delta_.setZero(); + if (!evaluator_->Plus(x_.data(), delta_.data(), candidate_x_.data())) { + solver_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; + solver_summary_->termination_type = FAILURE; + return false; } - 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; + x_ = candidate_x_; + x_norm_ = x_.norm(); } - 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; + if (!EvaluateGradientAndJacobian()) { + return false; } - 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; - } + solver_summary_->initial_cost = x_cost_ + solver_summary_->fixed_cost; + iteration_summary_.step_is_valid = true; + iteration_summary_.step_is_successful = true; + return true; +} - if (options_.jacobi_scaling) { - EstimateScale(*jacobian, scale.data()); - jacobian->ScaleColumns(scale.data()); - } else { - scale.setOnes(); +// For the current x_, compute +// +// 1. Cost +// 2. Jacobian +// 3. Gradient +// 4. Scale the Jacobian if needed (and compute the scaling if we are +// in iteration zero). +// 5. Compute the 2 and max norm of the gradient. +// +// Returns true if all computations could be performed +// successfully. Any failures are considered fatal and the +// Solver::Summary is updated to indicate this. +bool TrustRegionMinimizer::EvaluateGradientAndJacobian() { + if (!evaluator_->Evaluate(x_.data(), + &x_cost_, + residuals_.data(), + gradient_.data(), + jacobian_)) { + solver_summary_->message = "Residual and Jacobian evaluation failed."; + solver_summary_->termination_type = FAILURE; + return false; } - 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_summary_.cost = x_cost_ + solver_summary_->fixed_cost; - 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; + if (options_.jacobi_scaling) { + if (iteration_summary_.iteration == 0) { + // Compute a scaling vector that is used to improve the + // conditioning of the Jacobian. + // + // jacobian_scaling_ = diag(J'J)^{-1} + jacobian_->SquaredColumnNorm(jacobian_scaling_.data()); + for (int i = 0; i < jacobian_->num_cols(); ++i) { + // Add one to the denominator to prevent division by zero. + jacobian_scaling_[i] = 1.0 / (1.0 + sqrt(jacobian_scaling_[i])); + } } - 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; - } + // jacobian = jacobian * diag(J'J) ^{-1} + jacobian_->ScaleColumns(jacobian_scaling_.data()); + } + + // The gradient exists in the local tangent space. To account for + // the bounds constraints correctly, instead of just computing the + // norm of the gradient vector, we compute + // + // |Plus(x, -gradient) - x| + // + // Where the Plus operator lifts the negative gradient to the + // ambient space, adds it to x and projects it on the hypercube + // defined by the bounds. + negative_gradient_ = -gradient_; + if (!evaluator_->Plus(x_.data(), + negative_gradient_.data(), + projected_gradient_step_.data())) { + solver_summary_->message = + "projected_gradient_step = Plus(x, -gradient) failed."; + solver_summary_->termination_type = FAILURE; + return false; + } - const double strategy_start_time = WallTimeInSeconds(); - TrustRegionStrategy::PerSolveOptions per_solve_options; - per_solve_options.eta = options_.eta; - if (find(options_.trust_region_minimizer_iterations_to_dump.begin(), - options_.trust_region_minimizer_iterations_to_dump.end(), - iteration_summary.iteration) != - options_.trust_region_minimizer_iterations_to_dump.end()) { - per_solve_options.dump_format_type = - options_.trust_region_problem_dump_format_type; - per_solve_options.dump_filename_base = - JoinPath(options_.trust_region_problem_dump_directory, - StringPrintf("ceres_solver_iteration_%03d", - iteration_summary.iteration)); + iteration_summary_.gradient_max_norm = + (x_ - projected_gradient_step_).lpNorm<Eigen::Infinity>(); + iteration_summary_.gradient_norm = (x_ - projected_gradient_step_).norm(); + return true; +} + +// 1. Add the final timing information to the iteration summary. +// 2. Run the callbacks +// 3. Check for termination based on +// a. Run time +// b. Iteration count +// c. Max norm of the gradient +// d. Size of the trust region radius. +// +// Returns true if user did not terminate the solver and none of these +// termination criterion are met. +bool TrustRegionMinimizer::FinalizeIterationAndCheckIfMinimizerCanContinue() { + if (iteration_summary_.step_is_successful) { + ++solver_summary_->num_successful_steps; + if (x_cost_ < minimum_cost_) { + minimum_cost_ = x_cost_; + VectorRef(parameters_, num_parameters_) = x_; + iteration_summary_.step_is_nonmonotonic = false; } else { - per_solve_options.dump_format_type = TEXTFILE; - per_solve_options.dump_filename_base.clear(); + iteration_summary_.step_is_nonmonotonic = true; } + } else { + ++solver_summary_->num_unsuccessful_steps; + } - 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_.trust_region_radius = strategy_->Radius(); + iteration_summary_.iteration_time_in_seconds = + WallTimeInSeconds() - iteration_start_time_in_secs_; + iteration_summary_.cumulative_time_in_seconds = + WallTimeInSeconds() - start_time_in_secs_ + + solver_summary_->preprocessor_time_in_seconds; - 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; - } - } + solver_summary_->iterations.push_back(iteration_summary_); - 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; - } + if (!RunCallbacks(options_, iteration_summary_, solver_summary_)) { + return false; + } - // 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; + if (MaxSolverTimeReached()) { + return false; + } - // Undo the Jacobian column scaling. - delta = (trust_region_step.array() * scale.array()).matrix(); + if (MaxSolverIterationsReached()) { + return false; + } - // 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; - } - } + if (GradientToleranceReached()) { + return false; + } - 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 (MinTrustRegionRadiusReached()) { + return false; + } - 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; - } - } + return true; +} - 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; - } +// Compute the trust region step using the TrustRegionStrategy chosen +// by the user. +// +// If the strategy returns with LINEAR_SOLVER_FATAL_ERROR, which +// indicates an unrecoverable error, return false. This is the only +// condition that returns false. +// +// If the strategy returns with LINEAR_SOLVER_FAILURE, which indicates +// a numerical failure that could be recovered from by retrying +// (e.g. by increasing the strength of the regularization), we set +// iteration_summary_.step_is_valid to false and return true. +// +// In all other cases, we compute the decrease in the trust region +// model problem. In exact arithmetic, this should always be +// positive, but due to numerical problems in the TrustRegionStrategy +// or round off error when computing the decrease it may be +// negative. In which case again, we set +// iteration_summary_.step_is_valid to false. +bool TrustRegionMinimizer::ComputeTrustRegionStep() { + const double strategy_start_time = WallTimeInSeconds(); + iteration_summary_.step_is_valid = false; + 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)); + } - 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; - } + 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) { + solver_summary_->message = + "Linear solver failed due to unrecoverable " + "non-numeric causes. Please see the error log for clues. "; + solver_summary_->termination_type = FAILURE; + return false; + } - const double relative_decrease = - iteration_summary.cost_change / model_cost_change; + iteration_summary_.step_solver_time_in_seconds = + WallTimeInSeconds() - strategy_start_time; + iteration_summary_.linear_solver_iterations = strategy_summary.num_iterations; - const double historical_relative_decrease = - (reference_cost - new_cost) / - (accumulated_reference_model_cost_change + model_cost_change); + if (strategy_summary.termination_type == LINEAR_SOLVER_FAILURE) { + return true; + } - // 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; - } - } - } + // 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 + // = -(J * step)'(f + 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); + + // TODO(sameeragarwal) + // + // 1. What happens if model_cost_change_ = 0 + // 2. What happens if -epsilon <= model_cost_change_ < 0 for some + // small epsilon due to round off error. + iteration_summary_.step_is_valid = (model_cost_change_ > 0.0); + if (iteration_summary_.step_is_valid) { + // Undo the Jacobian column scaling. + delta_ = (trust_region_step_.array() * jacobian_scaling_.array()).matrix(); + num_consecutive_invalid_steps_ = 0; + } - 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; - } + VLOG_IF(1, is_not_silent_ && !iteration_summary_.step_is_valid) + << "Invalid step: current_cost: " << x_cost_ + << " absolute model cost change: " << model_cost_change_ + << " relative model cost change: " << (model_cost_change_ / x_cost_); + return true; +} - 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; - } +// Invalid steps can happen due to a number of reasons, and we allow a +// limited number of consecutive failures, and return false if this +// limit is exceeded. +bool TrustRegionMinimizer::HandleInvalidStep() { + // TODO(sameeragarwal): Should we be returning FAILURE or + // NO_CONVERGENCE? The solution value is still usable in many cases, + // it is not clear if we should declare the solver a failure + // entirely. For example the case where model_cost_change ~ 0.0, but + // just slightly negative. + if (++num_consecutive_invalid_steps_ >= + options_.max_num_consecutive_invalid_steps) { + solver_summary_->message = StringPrintf( + "Number of consecutive invalid steps more " + "than Solver::Options::max_num_consecutive_invalid_steps: %d", + options_.max_num_consecutive_invalid_steps); + solver_summary_->termination_type = FAILURE; + return false; + } - iteration_summary.gradient_max_norm = - (x - projected_gradient_step).lpNorm<Eigen::Infinity>(); - iteration_summary.gradient_norm = (x - projected_gradient_step).norm(); + strategy_->StepIsInvalid(); + + // 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 = x_cost_ + solver_summary_->fixed_cost; + iteration_summary_.cost_change = 0.0; + iteration_summary_.gradient_max_norm = + solver_summary_->iterations.back().gradient_max_norm; + iteration_summary_.gradient_norm = + solver_summary_->iterations.back().gradient_norm; + iteration_summary_.step_norm = 0.0; + iteration_summary_.relative_decrease = 0.0; + iteration_summary_.eta = options_.eta; + return true; +} - if (options_.jacobi_scaling) { - jacobian->ScaleColumns(scale.data()); - } +// Use the supplied coordinate descent minimizer to perform inner +// iterations and compute the improvement due to it. Returns the cost +// after performing the inner iterations. +// +// The optimization is performed with candidate_x_ as the starting +// point, and if the optimization is successful, candidate_x_ will be +// updated with the optimized parameters. +void TrustRegionMinimizer::DoInnerIterationsIfNeeded() { + inner_iterations_were_useful_ = false; + if (!inner_iterations_are_enabled_ || + candidate_cost_ >= std::numeric_limits<double>::max()) { + return; + } - // 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(); - } - } + double inner_iteration_start_time = WallTimeInSeconds(); + ++solver_summary_->num_inner_iteration_steps; + inner_iteration_x_ = candidate_x_; + Solver::Summary inner_iteration_summary; + options_.inner_iteration_minimizer->Minimize( + options_, inner_iteration_x_.data(), &inner_iteration_summary); + double inner_iteration_cost; + if (!evaluator_->Evaluate( + inner_iteration_x_.data(), &inner_iteration_cost, NULL, NULL, NULL)) { + VLOG_IF(2, is_not_silent_) << "Inner iteration failed."; + return; + } - 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; - } - } + VLOG_IF(2, is_not_silent_) + << "Inner iteration succeeded; Current cost: " << x_cost_ + << " Trust region step cost: " << candidate_cost_ + << " Inner iteration cost: " << inner_iteration_cost; + + candidate_x_ = inner_iteration_x_; + + // 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) + // + // Practically we do this by increasing model_cost_change by + // inner_iteration_cost_change. + + const double inner_iteration_cost_change = + candidate_cost_ - inner_iteration_cost; + model_cost_change_ += inner_iteration_cost_change; + inner_iterations_were_useful_ = inner_iteration_cost < x_cost_; + const double inner_iteration_relative_progress = + 1.0 - inner_iteration_cost / candidate_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; + candidate_cost_ = inner_iteration_cost; + + solver_summary_->inner_iteration_time_in_seconds += + WallTimeInSeconds() - inner_iteration_start_time; +} + +// Perform a projected line search to improve the objective function +// value along delta. +// +// TODO(sameeragarwal): The current implementation does not do +// anything illegal but is incorrect and not terribly effective. +// +// https://github.com/ceres-solver/ceres-solver/issues/187 +void TrustRegionMinimizer::DoLineSearch(const Vector& x, + const Vector& gradient, + const double cost, + Vector* delta) { + 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 line_search_summary; + line_search_function.Init(x, *delta); + line_search->Search(1.0, cost, gradient.dot(*delta), &line_search_summary); + + solver_summary_->num_line_search_steps += line_search_summary.num_iterations; + solver_summary_->line_search_cost_evaluation_time_in_seconds += + line_search_summary.cost_evaluation_time_in_seconds; + solver_summary_->line_search_gradient_evaluation_time_in_seconds += + line_search_summary.gradient_evaluation_time_in_seconds; + solver_summary_->line_search_polynomial_minimization_time_in_seconds += + line_search_summary.polynomial_minimization_time_in_seconds; + solver_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; + } +} + +// Check if the maximum amount of time allowed by the user for the +// solver has been exceeded, and if so return false after updating +// Solver::Summary::message. +bool TrustRegionMinimizer::MaxSolverTimeReached() { + const double total_solver_time = + WallTimeInSeconds() - start_time_in_secs_ + + solver_summary_->preprocessor_time_in_seconds; + if (total_solver_time < options_.max_solver_time_in_seconds) { + return false; + } + + solver_summary_->message = StringPrintf("Maximum solver time reached. " + "Total solver time: %e >= %e.", + total_solver_time, + options_.max_solver_time_in_seconds); + solver_summary_->termination_type = NO_CONVERGENCE; + VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message; + return true; +} + +// Check if the maximum number of iterations allowed by the user for +// the solver has been exceeded, and if so return false after updating +// Solver::Summary::message. +bool TrustRegionMinimizer::MaxSolverIterationsReached() { + if (iteration_summary_.iteration < options_.max_num_iterations) { + return false; + } + + solver_summary_->message = + StringPrintf("Maximum number of iterations reached. " + "Number of iterations: %d.", + iteration_summary_.iteration); + + solver_summary_->termination_type = NO_CONVERGENCE; + VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message; + return true; +} + +// Check convergence based on the max norm of the gradient (only for +// iterations where the step was declared successful). +bool TrustRegionMinimizer::GradientToleranceReached() { + if (!iteration_summary_.step_is_successful || + iteration_summary_.gradient_max_norm > options_.gradient_tolerance) { + return false; + } + + solver_summary_->message = StringPrintf( + "Gradient tolerance reached. " + "Gradient max norm: %e <= %e", + iteration_summary_.gradient_max_norm, + options_.gradient_tolerance); + solver_summary_->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message; + return true; +} + +// Check convergence based the size of the trust region radius. +bool TrustRegionMinimizer::MinTrustRegionRadiusReached() { + if (iteration_summary_.trust_region_radius > + options_.min_trust_region_radius) { + return false; + } + + solver_summary_->message = + StringPrintf("Minimum trust region radius reached. " + "Trust region radius: %e <= %e", + iteration_summary_.trust_region_radius, + options_.min_trust_region_radius); + solver_summary_->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message; + return true; +} + +// Solver::Options::parameter_tolerance based convergence check. +bool TrustRegionMinimizer::ParameterToleranceReached() { + // Compute the norm of the step in the ambient space. + iteration_summary_.step_norm = (x_ - candidate_x_).norm(); + const double step_size_tolerance = + options_.parameter_tolerance * (x_norm_ + options_.parameter_tolerance); + + if (iteration_summary_.step_norm > step_size_tolerance) { + return false; } + + solver_summary_->message = StringPrintf( + "Parameter tolerance reached. " + "Relative step_norm: %e <= %e.", + (iteration_summary_.step_norm / (x_norm_ + options_.parameter_tolerance)), + options_.parameter_tolerance); + solver_summary_->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message; + return true; +} + +// Solver::Options::function_tolerance based convergence check. +bool TrustRegionMinimizer::FunctionToleranceReached() { + iteration_summary_.cost_change = x_cost_ - candidate_cost_; + const double absolute_function_tolerance = + options_.function_tolerance * x_cost_; + + if (fabs(iteration_summary_.cost_change) > absolute_function_tolerance) { + return false; + } + + solver_summary_->message = StringPrintf( + "Function tolerance reached. " + "|cost_change|/cost: %e <= %e", + fabs(iteration_summary_.cost_change) / x_cost_, + options_.function_tolerance); + solver_summary_->termination_type = CONVERGENCE; + VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message; + return true; } +// Compute candidate_x_ = Plus(x_, delta_) +// Evaluate the cost of candidate_x_ as candidate_cost_. +// +// Failure to compute the step or the cost mean that candidate_cost_ +// is set to std::numeric_limits<double>::max(). Unlike +// EvaluateGradientAndJacobian, failure in this function is not fatal +// as we are only computing and evaluating a candidate point, and if +// for some reason we are unable to evaluate it, we consider it to be +// a point with very high cost. This allows the user to deal with edge +// cases/constraints as part of the LocalParameterization and +// CostFunction objects. +void TrustRegionMinimizer::ComputeCandidatePointAndEvaluateCost() { + if (!evaluator_->Plus(x_.data(), delta_.data(), candidate_x_.data())) { + LOG_IF(WARNING, is_not_silent_) + << "x_plus_delta = Plus(x, delta) failed. " + << "Treating it as a step with infinite cost"; + candidate_cost_ = std::numeric_limits<double>::max(); + return; + } + + if (!evaluator_->Evaluate( + candidate_x_.data(), &candidate_cost_, NULL, NULL, NULL)) { + LOG_IF(WARNING, is_not_silent_) + << "Step failed to evaluate. " + << "Treating it as a step with infinite cost"; + candidate_cost_ = std::numeric_limits<double>::max(); + } +} + +bool TrustRegionMinimizer::IsStepSuccessful() { + iteration_summary_.relative_decrease = + step_evaluator_->StepQuality(candidate_cost_, model_cost_change_); + + // In most cases, boosting the model_cost_change by the + // improvement caused by the inner iterations is fine, but it can + // be the case that the original trust region step was so bad that + // the resulting improvement in the cost was negative, and the + // change caused by the inner iterations was large enough to + // improve the step, but also to make relative decrease 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. + return (inner_iterations_were_useful_ || + iteration_summary_.relative_decrease > + options_.min_relative_decrease); +} + +// Declare the step successful, move to candidate_x, update the +// derivatives and let the trust region strategy and the step +// evaluator know that the step has been accepted. +bool TrustRegionMinimizer::HandleSuccessfulStep() { + x_ = candidate_x_; + x_norm_ = x_.norm(); + + if (!EvaluateGradientAndJacobian()) { + return false; + } + + iteration_summary_.step_is_successful = true; + strategy_->StepAccepted(iteration_summary_.relative_decrease); + step_evaluator_->StepAccepted(candidate_cost_, model_cost_change_); + return true; +} + +// Declare the step unsuccessful and inform the trust region strategy. +void TrustRegionMinimizer::HandleUnsuccessfulStep() { + iteration_summary_.step_is_successful = false; + strategy_->StepRejected(iteration_summary_.relative_decrease); + iteration_summary_.cost = candidate_cost_ + solver_summary_->fixed_cost; +} } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/trust_region_minimizer.h b/extern/ceres/internal/ceres/trust_region_minimizer.h index ed52c2642d1..43141da58a1 100644 --- a/extern/ceres/internal/ceres/trust_region_minimizer.h +++ b/extern/ceres/internal/ceres/trust_region_minimizer.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2016 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,35 +31,136 @@ #ifndef CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ #define CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" #include "ceres/minimizer.h" #include "ceres/solver.h" +#include "ceres/sparse_matrix.h" +#include "ceres/trust_region_step_evaluator.h" +#include "ceres/trust_region_strategy.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. +// Generic trust region minimization algorithm. // // For example usage, see SolverImpl::Minimize. class TrustRegionMinimizer : public Minimizer { public: - ~TrustRegionMinimizer() {} + ~TrustRegionMinimizer(); + + // This method is not thread safe. virtual void Minimize(const Minimizer::Options& options, double* parameters, - Solver::Summary* summary); + Solver::Summary* solver_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; + void Init(const Minimizer::Options& options, + double* parameters, + Solver::Summary* solver_summary); + bool IterationZero(); + bool FinalizeIterationAndCheckIfMinimizerCanContinue(); + bool ComputeTrustRegionStep(); + + bool EvaluateGradientAndJacobian(); + void ComputeCandidatePointAndEvaluateCost(); + + void DoLineSearch(const Vector& x, + const Vector& gradient, + const double cost, + Vector* delta); + void DoInnerIterationsIfNeeded(); + + bool ParameterToleranceReached(); + bool FunctionToleranceReached(); + bool GradientToleranceReached(); + bool MaxSolverTimeReached(); + bool MaxSolverIterationsReached(); + bool MinTrustRegionRadiusReached(); + + bool IsStepSuccessful(); + void HandleUnsuccessfulStep(); + bool HandleSuccessfulStep(); + bool HandleInvalidStep(); Minimizer::Options options_; + + // These pointers are shortcuts to objects passed to the + // TrustRegionMinimizer. The TrustRegionMinimizer does not own them. + double* parameters_; + Solver::Summary* solver_summary_; + Evaluator* evaluator_; + SparseMatrix* jacobian_; + TrustRegionStrategy* strategy_; + + scoped_ptr<TrustRegionStepEvaluator> step_evaluator_; + + bool is_not_silent_; + bool inner_iterations_are_enabled_; + bool inner_iterations_were_useful_; + + // Summary of the current iteration. + IterationSummary iteration_summary_; + + // Dimensionality of the problem in the ambient space. + int num_parameters_; + // Dimensionality of the problem in the tangent space. This is the + // number of columns in the Jacobian. + int num_effective_parameters_; + // Length of the residual vector, also the number of rows in the Jacobian. + int num_residuals_; + + // Current point. + Vector x_; + // Residuals at x_; + Vector residuals_; + // Gradient at x_. + Vector gradient_; + // Solution computed by the inner iterations. + Vector inner_iteration_x_; + // model_residuals = J * trust_region_step + Vector model_residuals_; + Vector negative_gradient_; + // projected_gradient_step = Plus(x, -gradient), an intermediate + // quantity used to compute the projected gradient norm. + Vector projected_gradient_step_; + // The step computed by the trust region strategy. If Jacobi scaling + // is enabled, this is a vector in the scaled space. + Vector trust_region_step_; + // The current proposal for how far the trust region algorithm + // thinks we should move. In the most basic case, it is just the + // trust_region_step_ with the Jacobi scaling undone. If bounds + // constraints are present, then it is the result of the projected + // line search. + Vector delta_; + // candidate_x = Plus(x, delta) + Vector candidate_x_; + // Scaling vector to scale the columns of the Jacobian. + Vector jacobian_scaling_; + + // Euclidean norm of x_. + double x_norm_; + // Cost at x_. + double x_cost_; + // Minimum cost encountered up till now. + double minimum_cost_; + // How much did the trust region strategy reduce the cost of the + // linearized Gauss-Newton model. + double model_cost_change_; + // Cost at candidate_x_. + double candidate_cost_; + + // Time at which the minimizer was started. + double start_time_in_secs_; + // Time at which the current iteration was started. + double iteration_start_time_in_secs_; + // Number of consecutive steps where the minimizer loop computed a + // numerically invalid step. + int num_consecutive_invalid_steps_; }; } // namespace internal } // namespace ceres + #endif // CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ diff --git a/extern/ceres/internal/ceres/trust_region_step_evaluator.cc b/extern/ceres/internal/ceres/trust_region_step_evaluator.cc new file mode 100644 index 00000000000..c9167e623ef --- /dev/null +++ b/extern/ceres/internal/ceres/trust_region_step_evaluator.cc @@ -0,0 +1,107 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 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 "ceres/trust_region_step_evaluator.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +TrustRegionStepEvaluator::TrustRegionStepEvaluator( + const double initial_cost, + const int max_consecutive_nonmonotonic_steps) + : max_consecutive_nonmonotonic_steps_(max_consecutive_nonmonotonic_steps), + minimum_cost_(initial_cost), + current_cost_(initial_cost), + reference_cost_(initial_cost), + candidate_cost_(initial_cost), + accumulated_reference_model_cost_change_(0.0), + accumulated_candidate_model_cost_change_(0.0), + num_consecutive_nonmonotonic_steps_(0){ +} + +double TrustRegionStepEvaluator::StepQuality( + const double cost, + const double model_cost_change) const { + const double relative_decrease = (current_cost_ - cost) / model_cost_change; + const double historical_relative_decrease = + (reference_cost_ - cost) / + (accumulated_reference_model_cost_change_ + model_cost_change); + return std::max(relative_decrease, historical_relative_decrease); +} + +void TrustRegionStepEvaluator::StepAccepted( + const double cost, + const double model_cost_change) { + // Algorithm 10.1.2 from Trust Region Methods by Conn, Gould & + // Toint. + // + // Step 3a + current_cost_ = cost; + accumulated_candidate_model_cost_change_ += model_cost_change; + accumulated_reference_model_cost_change_ += model_cost_change; + + // Step 3b. + if (current_cost_ < minimum_cost_) { + minimum_cost_ = current_cost_; + num_consecutive_nonmonotonic_steps_ = 0; + candidate_cost_ = current_cost_; + accumulated_candidate_model_cost_change_ = 0.0; + } else { + // Step 3c. + ++num_consecutive_nonmonotonic_steps_; + if (current_cost_ > candidate_cost_) { + candidate_cost_ = current_cost_; + accumulated_candidate_model_cost_change_ = 0.0; + } + } + + // Step 3d. + // + // 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. + // + // Note: In the original algorithm by Toint, this step was only + // executed if the step was non-monotonic, but that would not handle + // the case of max_consecutive_nonmonotonic_steps = 0. The small + // modification of doing this always handles that corner case + // correctly. + if (num_consecutive_nonmonotonic_steps_ == + max_consecutive_nonmonotonic_steps_) { + reference_cost_ = candidate_cost_; + accumulated_reference_model_cost_change_ = + accumulated_candidate_model_cost_change_; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/trust_region_step_evaluator.h b/extern/ceres/internal/ceres/trust_region_step_evaluator.h new file mode 100644 index 00000000000..06df102a308 --- /dev/null +++ b/extern/ceres/internal/ceres/trust_region_step_evaluator.h @@ -0,0 +1,122 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 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_STEP_EVALUATOR_H_ +#define CERES_INTERNAL_TRUST_REGION_STEP_EVALUATOR_H_ + +namespace ceres { +namespace internal { + +// The job of the TrustRegionStepEvaluator is to evaluate the quality +// of a step, i.e., how the cost of a step compares with the reduction +// in the objective of the trust region problem. +// +// Classic trust region methods are descent methods, in that they only +// accept a point if it strictly reduces the value of the objective +// function. They do this by measuring the quality of a step as +// +// cost_change / model_cost_change. +// +// Relaxing the monotonic descent 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 principled manner allows the algorithm to "jump over +// boulders" as the method is not restricted to move into narrow +// valleys while preserving its convergence properties. +// +// The parameter max_consecutive_nonmonotonic_steps controls the +// window size used by the step selection algorithm to accept +// non-monotonic steps. Setting this parameter to zero, recovers the +// classic montonic descent algorithm. +// +// 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. +// +// Example usage: +// +// TrustRegionStepEvaluator* step_evaluator = ... +// +// cost = ... // Compute the non-linear objective function value. +// model_cost_change = ... // Change in the value of the trust region objective. +// if (step_evaluator->StepQuality(cost, model_cost_change) > threshold) { +// x = x + delta; +// step_evaluator->StepAccepted(cost, model_cost_change); +// } +class TrustRegionStepEvaluator { + public: + // initial_cost is as the name implies the cost of the starting + // state of the trust region minimizer. + // + // max_consecutive_nonmonotonic_steps controls the window size used + // by the step selection algorithm to accept non-monotonic + // steps. Setting this parameter to zero, recovers the classic + // montonic descent algorithm. + TrustRegionStepEvaluator(double initial_cost, + int max_consecutive_nonmonotonic_steps); + + // Return the quality of the step given its cost and the decrease in + // the cost of the model. model_cost_change has to be positive. + double StepQuality(double cost, double model_cost_change) const; + + // Inform the step evaluator that a step with the given cost and + // model_cost_change has been accepted by the trust region + // minimizer. + void StepAccepted(double cost, double model_cost_change); + + private: + const int max_consecutive_nonmonotonic_steps_; + // The minimum cost encountered up till now. + double minimum_cost_; + // The current cost of the trust region minimizer as informed by the + // last call to StepAccepted. + double current_cost_; + double reference_cost_; + double candidate_cost_; + // Accumulated model cost since the last time the reference model + // cost was updated, i.e., when a step with cost less than the + // current known minimum cost is accepted. + double accumulated_reference_model_cost_change_; + // Accumulated model cost since the last time the candidate model + // cost was updated, i.e., a non-monotonic step was taken with a + // cost that was greater than the current candidate cost. + double accumulated_candidate_model_cost_change_; + // Number of steps taken since the last time minimum_cost was updated. + int num_consecutive_nonmonotonic_steps_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_TRUST_REGION_STEP_EVALUATOR_H_ diff --git a/extern/ceres/internal/ceres/trust_region_strategy.h b/extern/ceres/internal/ceres/trust_region_strategy.h index 9560e67459a..36e8e981cc0 100644 --- a/extern/ceres/internal/ceres/trust_region_strategy.h +++ b/extern/ceres/internal/ceres/trust_region_strategy.h @@ -86,20 +86,20 @@ class TrustRegionStrategy { struct PerSolveOptions { PerSolveOptions() : eta(0), - dump_filename_base(""), dump_format_type(TEXTFILE) { } // Forcing sequence for inexact solves. double eta; + DumpFormatType dump_format_type; + // 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 { |