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/ceres | |
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/ceres')
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 { |