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/include | |
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/include')
-rw-r--r-- | extern/ceres/include/ceres/cost_function_to_functor.h | 3 | ||||
-rw-r--r-- | extern/ceres/include/ceres/covariance.h | 56 | ||||
-rw-r--r-- | extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h | 22 | ||||
-rw-r--r-- | extern/ceres/include/ceres/gradient_checker.h | 239 | ||||
-rw-r--r-- | extern/ceres/include/ceres/internal/port.h | 22 | ||||
-rw-r--r-- | extern/ceres/include/ceres/iteration_callback.h | 6 | ||||
-rw-r--r-- | extern/ceres/include/ceres/jet.h | 106 | ||||
-rw-r--r-- | extern/ceres/include/ceres/local_parameterization.h | 22 | ||||
-rw-r--r-- | extern/ceres/include/ceres/numeric_diff_cost_function.h | 23 | ||||
-rw-r--r-- | extern/ceres/include/ceres/problem.h | 7 | ||||
-rw-r--r-- | extern/ceres/include/ceres/rotation.h | 3 | ||||
-rw-r--r-- | extern/ceres/include/ceres/solver.h | 31 | ||||
-rw-r--r-- | extern/ceres/include/ceres/version.h | 2 |
13 files changed, 309 insertions, 233 deletions
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 |