diff options
Diffstat (limited to 'extern/ceres/include')
54 files changed, 4736 insertions, 3326 deletions
diff --git a/extern/ceres/include/ceres/autodiff_cost_function.h b/extern/ceres/include/ceres/autodiff_cost_function.h index e7893e4828e..5e6e9c55db5 100644 --- a/extern/ceres/include/ceres/autodiff_cost_function.h +++ b/extern/ceres/include/ceres/autodiff_cost_function.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -30,7 +30,7 @@ // // Create CostFunctions as needed by the least squares framework, with // Jacobians computed via automatic differentiation. For more -// information on automatic differentation, see the wikipedia article +// information on automatic differentiation, see the wikipedia article // at http://en.wikipedia.org/wiki/Automatic_differentiation // // To get an auto differentiated cost function, you must define a class with a @@ -54,7 +54,7 @@ // for a series of measurements, where there is an instance of the cost function // for each measurement k. // -// The actual cost added to the total problem is e^2, or (k - x'k)^2; however, +// The actual cost added to the total problem is e^2, or (k - x'y)^2; however, // the squaring is implicitly done by the optimization framework. // // To write an auto-differentiable cost function for the above model, first @@ -90,7 +90,7 @@ // Dimension of x ---------------+ | // Dimension of y ------------------+ // -// In this example, there is usually an instance for each measumerent of k. +// In this example, there is usually an instance for each measurement of k. // // In the instantiation above, the template parameters following // "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing a @@ -110,12 +110,8 @@ // Dimension of x ------------------------------------+ | // Dimension of y ---------------------------------------+ // -// The framework can currently accommodate cost functions of up to 10 -// independent variables, and there is no limit on the dimensionality -// of each of them. -// // WARNING #1: Since the functor will get instantiated with different types for -// T, you must to convert from other numeric types to T before mixing +// T, you must convert from other numeric types to T before mixing // computations with other variables of type T. In the example above, this is // seen where instead of using k_ directly, k_ is wrapped with T(k_). // @@ -129,8 +125,9 @@ #ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ #define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ +#include <memory> + #include "ceres/internal/autodiff.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/sized_cost_function.h" #include "ceres/types.h" #include "glog/logging.h" @@ -138,7 +135,7 @@ namespace ceres { // A cost function which computes the derivative of the cost with respect to -// the parameters (a.k.a. the jacobian) using an autodifferentiation framework. +// the parameters (a.k.a. the jacobian) using an auto differentiation framework. // The first template argument is the functor object, described in the header // comment. The second argument is the dimension of the residual (or // ceres::DYNAMIC to indicate it will be set at runtime), and subsequent @@ -153,27 +150,15 @@ namespace ceres { // of residuals for a single autodiff cost function at runtime. template <typename CostFunctor, int kNumResiduals, // Number of residuals, or ceres::DYNAMIC. - int N0, // Number of parameters in block 0. - int N1 = 0, // Number of parameters in block 1. - int N2 = 0, // Number of parameters in block 2. - int N3 = 0, // Number of parameters in block 3. - int N4 = 0, // Number of parameters in block 4. - int N5 = 0, // Number of parameters in block 5. - int N6 = 0, // Number of parameters in block 6. - int N7 = 0, // Number of parameters in block 7. - int N8 = 0, // Number of parameters in block 8. - int N9 = 0> // Number of parameters in block 9. -class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, - N0, N1, N2, N3, N4, - N5, N6, N7, N8, N9> { + int... Ns> // Number of parameters in each parameter block. +class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> { public: // Takes ownership of functor. Uses the template-provided value for the // number of residuals ("kNumResiduals"). - explicit AutoDiffCostFunction(CostFunctor* functor) - : functor_(functor) { - CHECK_NE(kNumResiduals, DYNAMIC) - << "Can't run the fixed-size constructor if the " - << "number of residuals is set to ceres::DYNAMIC."; + explicit AutoDiffCostFunction(CostFunctor* functor) : functor_(functor) { + static_assert(kNumResiduals != DYNAMIC, + "Can't run the fixed-size constructor if the number of " + "residuals is set to ceres::DYNAMIC."); } // Takes ownership of functor. Ignores the template-provided @@ -183,13 +168,10 @@ class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, // numbers of residuals at runtime. AutoDiffCostFunction(CostFunctor* functor, int num_residuals) : functor_(functor) { - CHECK_EQ(kNumResiduals, DYNAMIC) - << "Can't run the dynamic-size constructor if the " - << "number of residuals is not ceres::DYNAMIC."; - SizedCostFunction<kNumResiduals, - N0, N1, N2, N3, N4, - N5, N6, N7, N8, N9> - ::set_num_residuals(num_residuals); + static_assert(kNumResiduals == DYNAMIC, + "Can't run the dynamic-size constructor if the number of " + "residuals is not ceres::DYNAMIC."); + SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals); } virtual ~AutoDiffCostFunction() {} @@ -197,29 +179,28 @@ class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, // Implementation details follow; clients of the autodiff cost function should // not have to examine below here. // - // To handle varardic cost functions, some template magic is needed. It's + // To handle variadic cost functions, some template magic is needed. It's // mostly hidden inside autodiff.h. - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override { + using ParameterDims = + typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims; + if (!jacobians) { - return internal::VariadicEvaluate< - CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> - ::Call(*functor_, parameters, residuals); + return internal::VariadicEvaluate<ParameterDims>( + *functor_, parameters, residuals); } - return internal::AutoDiff<CostFunctor, double, - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate( - *functor_, - parameters, - SizedCostFunction<kNumResiduals, - N0, N1, N2, N3, N4, - N5, N6, N7, N8, N9>::num_residuals(), - residuals, - jacobians); - } + return internal::AutoDifferentiate<kNumResiduals, ParameterDims>( + *functor_, + parameters, + SizedCostFunction<kNumResiduals, Ns...>::num_residuals(), + residuals, + jacobians); + }; private: - internal::scoped_ptr<CostFunctor> functor_; + std::unique_ptr<CostFunctor> functor_; }; } // namespace ceres diff --git a/extern/ceres/include/ceres/autodiff_first_order_function.h b/extern/ceres/include/ceres/autodiff_first_order_function.h new file mode 100644 index 00000000000..b98d845655b --- /dev/null +++ b/extern/ceres/include/ceres/autodiff_first_order_function.h @@ -0,0 +1,151 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_AUTODIFF_FIRST_ORDER_FUNCTION_H_ +#define CERES_PUBLIC_AUTODIFF_FIRST_ORDER_FUNCTION_H_ + +#include <memory> + +#include "ceres/first_order_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/jet.h" +#include "ceres/types.h" + +namespace ceres { + +// Create FirstOrderFunctions as needed by the GradientProblem +// framework, with gradients computed via automatic +// differentiation. For more information on automatic differentiation, +// see the wikipedia article at +// http://en.wikipedia.org/wiki/Automatic_differentiation +// +// To get an auto differentiated function, you must define a class +// with a templated operator() (a functor) that computes the cost +// function in terms of the template parameter T. The autodiff +// framework substitutes appropriate "jet" objects for T in order to +// compute the derivative when necessary, but this is hidden, and you +// should write the function as if T were a scalar type (e.g. a +// double-precision floating point number). +// +// The function must write the computed value in the last argument +// (the only non-const one) and return true to indicate +// success. +// +// For example, consider a scalar error e = x'y - a, where both x and y are +// two-dimensional column vector parameters, the prime sign indicates +// transposition, and a is a constant. +// +// To write an auto-differentiable FirstOrderFunction for the above model, first +// define the object +// +// class QuadraticCostFunctor { +// public: +// explicit QuadraticCostFunctor(double a) : a_(a) {} +// template <typename T> +// bool operator()(const T* const xy, T* cost) const { +// const T* const x = xy; +// const T* const y = xy + 2; +// *cost = x[0] * y[0] + x[1] * y[1] - T(a_); +// return true; +// } +// +// private: +// double a_; +// }; +// +// Note that in the declaration of operator() the input parameters xy come +// first, and are passed as const pointers to arrays of T. The +// output is the last parameter. +// +// Then given this class definition, the auto differentiated FirstOrderFunction +// for it can be constructed as follows. +// +// FirstOrderFunction* function = +// new AutoDiffFirstOrderFunction<QuadraticCostFunctor, 4>( +// new QuadraticCostFunctor(1.0))); +// +// In the instantiation above, the template parameters following +// "QuadraticCostFunctor", "4", describe the functor as computing a +// 1-dimensional output from a four dimensional vector. +// +// WARNING: Since the functor will get instantiated with different types for +// T, you must convert from other numeric types to T before mixing +// computations with other variables of type T. In the example above, this is +// seen where instead of using a_ directly, a_ is wrapped with T(a_). + +template <typename FirstOrderFunctor, int kNumParameters> +class AutoDiffFirstOrderFunction : public FirstOrderFunction { + public: + // Takes ownership of functor. + explicit AutoDiffFirstOrderFunction(FirstOrderFunctor* functor) + : functor_(functor) { + static_assert(kNumParameters > 0, "kNumParameters must be positive"); + } + + virtual ~AutoDiffFirstOrderFunction() {} + + bool Evaluate(const double* const parameters, + double* cost, + double* gradient) const override { + if (gradient == nullptr) { + return (*functor_)(parameters, cost); + } + + typedef Jet<double, kNumParameters> JetT; + internal::FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(kNumParameters); + for (int i = 0; i < kNumParameters; ++i) { + x[i].a = parameters[i]; + x[i].v.setZero(); + x[i].v[i] = 1.0; + } + + JetT output; + output.a = kImpossibleValue; + output.v.setConstant(kImpossibleValue); + + if (!(*functor_)(x.data(), &output)) { + return false; + } + + *cost = output.a; + VectorRef(gradient, kNumParameters) = output.v; + return true; + } + + int NumParameters() const override { return kNumParameters; } + + private: + std::unique_ptr<FirstOrderFunctor> functor_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_AUTODIFF_FIRST_ORDER_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/autodiff_local_parameterization.h b/extern/ceres/include/ceres/autodiff_local_parameterization.h index 27397e20d3b..d694376fdd1 100644 --- a/extern/ceres/include/ceres/autodiff_local_parameterization.h +++ b/extern/ceres/include/ceres/autodiff_local_parameterization.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -33,9 +33,10 @@ #ifndef CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ #define CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ -#include "ceres/local_parameterization.h" +#include <memory> + #include "ceres/internal/autodiff.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/local_parameterization.h" namespace ceres { @@ -107,21 +108,20 @@ namespace ceres { template <typename Functor, int kGlobalSize, int kLocalSize> class AutoDiffLocalParameterization : public LocalParameterization { public: - AutoDiffLocalParameterization() : - functor_(new Functor()) {} + AutoDiffLocalParameterization() : functor_(new Functor()) {} // Takes ownership of functor. - explicit AutoDiffLocalParameterization(Functor* functor) : - functor_(functor) {} + explicit AutoDiffLocalParameterization(Functor* functor) + : functor_(functor) {} virtual ~AutoDiffLocalParameterization() {} - virtual bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const { + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override { return (*functor_)(x, delta, x_plus_delta); } - virtual bool ComputeJacobian(const double* x, double* jacobian) const { + bool ComputeJacobian(const double* x, double* jacobian) const override { double zero_delta[kLocalSize]; for (int i = 0; i < kLocalSize; ++i) { zero_delta[i] = 0.0; @@ -133,20 +133,18 @@ class AutoDiffLocalParameterization : public LocalParameterization { } const double* parameter_ptrs[2] = {x, zero_delta}; - double* jacobian_ptrs[2] = { NULL, jacobian }; - return internal::AutoDiff<Functor, double, kGlobalSize, kLocalSize> - ::Differentiate(*functor_, - parameter_ptrs, - kGlobalSize, - x_plus_delta, - jacobian_ptrs); + double* jacobian_ptrs[2] = {NULL, jacobian}; + return internal::AutoDifferentiate< + kGlobalSize, + internal::StaticParameterDims<kGlobalSize, kLocalSize>>( + *functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); } - virtual int GlobalSize() const { return kGlobalSize; } - virtual int LocalSize() const { return kLocalSize; } + int GlobalSize() const override { return kGlobalSize; } + int LocalSize() const override { return kLocalSize; } private: - internal::scoped_ptr<Functor> functor_; + std::unique_ptr<Functor> functor_; }; } // namespace ceres diff --git a/extern/ceres/include/ceres/c_api.h b/extern/ceres/include/ceres/c_api.h index df7c9b6d671..0e6e590d0f5 100644 --- a/extern/ceres/include/ceres/c_api.h +++ b/extern/ceres/include/ceres/c_api.h @@ -1,5 +1,5 @@ /* Ceres Solver - A fast non-linear least squares minimizer - * Copyright 2015 Google Inc. All rights reserved. + * Copyright 2019 Google Inc. All rights reserved. * http://ceres-solver.org/ * * Redistribution and use in source and binary forms, with or without @@ -143,4 +143,4 @@ CERES_EXPORT void ceres_solve(ceres_problem_t* problem); #include "ceres/internal/reenable_warnings.h" -#endif /* CERES_PUBLIC_C_API_H_ */ +#endif /* CERES_PUBLIC_C_API_H_ */ diff --git a/extern/ceres/include/ceres/ceres.h b/extern/ceres/include/ceres/ceres.h index 64ffb99798a..d249351694c 100644 --- a/extern/ceres/include/ceres/ceres.h +++ b/extern/ceres/include/ceres/ceres.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -36,12 +36,18 @@ #include "ceres/autodiff_cost_function.h" #include "ceres/autodiff_local_parameterization.h" +#include "ceres/conditioned_cost_function.h" +#include "ceres/context.h" #include "ceres/cost_function.h" #include "ceres/cost_function_to_functor.h" #include "ceres/covariance.h" #include "ceres/crs_matrix.h" #include "ceres/dynamic_autodiff_cost_function.h" +#include "ceres/dynamic_cost_function.h" +#include "ceres/dynamic_cost_function_to_functor.h" #include "ceres/dynamic_numeric_diff_cost_function.h" +#include "ceres/evaluation_callback.h" +#include "ceres/gradient_checker.h" #include "ceres/gradient_problem.h" #include "ceres/gradient_problem_solver.h" #include "ceres/iteration_callback.h" @@ -49,6 +55,7 @@ #include "ceres/local_parameterization.h" #include "ceres/loss_function.h" #include "ceres/numeric_diff_cost_function.h" +#include "ceres/numeric_diff_options.h" #include "ceres/ordered_groups.h" #include "ceres/problem.h" #include "ceres/sized_cost_function.h" diff --git a/extern/ceres/include/ceres/conditioned_cost_function.h b/extern/ceres/include/ceres/conditioned_cost_function.h index 29597d935cb..a57ee209b80 100644 --- a/extern/ceres/include/ceres/conditioned_cost_function.h +++ b/extern/ceres/include/ceres/conditioned_cost_function.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -34,12 +34,12 @@ #ifndef CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ #define CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ +#include <memory> #include <vector> #include "ceres/cost_function.h" -#include "ceres/internal/scoped_ptr.h" -#include "ceres/types.h" #include "ceres/internal/disable_warnings.h" +#include "ceres/types.h" namespace ceres { @@ -77,17 +77,19 @@ class CERES_EXPORT ConditionedCostFunction : public CostFunction { // per-residual conditioner. Takes ownership of all of the wrapped cost // functions, or not, depending on the ownership parameter. Conditioners // may be NULL, in which case the corresponding residual is not modified. + // + // The conditioners can repeat. ConditionedCostFunction(CostFunction* wrapped_cost_function, const std::vector<CostFunction*>& conditioners, Ownership ownership); virtual ~ConditionedCostFunction(); - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const; + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override; private: - internal::scoped_ptr<CostFunction> wrapped_cost_function_; + std::unique_ptr<CostFunction> wrapped_cost_function_; std::vector<CostFunction*> conditioners_; Ownership ownership_; }; diff --git a/extern/ceres/include/ceres/context.h b/extern/ceres/include/ceres/context.h new file mode 100644 index 00000000000..d08e32b31a8 --- /dev/null +++ b/extern/ceres/include/ceres/context.h @@ -0,0 +1,56 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) + +#ifndef CERES_PUBLIC_CONTEXT_H_ +#define CERES_PUBLIC_CONTEXT_H_ + +namespace ceres { + +// A global context for processing data in Ceres. This provides a mechanism to +// allow Ceres to reuse items that are expensive to create between multiple +// calls; for example, thread pools. The same Context can be used on multiple +// Problems, either serially or in parallel. When using it with multiple +// Problems at the same time, they may end up contending for resources +// (e.g. threads) managed by the Context. +class Context { + public: + Context() {} + Context(const Context&) = delete; + void operator=(const Context&) = delete; + + virtual ~Context() {} + + // Creates a context object and the caller takes ownership. + static Context* Create(); +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_CONTEXT_H_ diff --git a/extern/ceres/include/ceres/cost_function.h b/extern/ceres/include/ceres/cost_function.h index f051a897c0d..d1550c119e8 100644 --- a/extern/ceres/include/ceres/cost_function.h +++ b/extern/ceres/include/ceres/cost_function.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -44,18 +44,18 @@ #ifndef CERES_PUBLIC_COST_FUNCTION_H_ #define CERES_PUBLIC_COST_FUNCTION_H_ +#include <cstdint> #include <vector> -#include "ceres/internal/macros.h" -#include "ceres/internal/port.h" -#include "ceres/types.h" + #include "ceres/internal/disable_warnings.h" +#include "ceres/internal/port.h" namespace ceres { // This class implements the computation of the cost (a.k.a. residual) terms as // a function of the input (control) variables, and is the interface for users // to describe their least squares problem to Ceres. In other words, this is the -// modelling layer between users and the Ceres optimizer. The signature of the +// modeling layer between users and the Ceres optimizer. The signature of the // function (number and sizes of input parameter blocks and number of outputs) // is stored in parameter_block_sizes_ and num_residuals_ respectively. User // code inheriting from this class is expected to set these two members with the @@ -64,6 +64,8 @@ namespace ceres { class CERES_EXPORT CostFunction { public: CostFunction() : num_residuals_(0) {} + CostFunction(const CostFunction&) = delete; + void operator=(const CostFunction&) = delete; virtual ~CostFunction() {} @@ -115,29 +117,24 @@ class CERES_EXPORT CostFunction { double* residuals, double** jacobians) const = 0; - const std::vector<int32>& parameter_block_sizes() const { + const std::vector<int32_t>& parameter_block_sizes() const { return parameter_block_sizes_; } - int num_residuals() const { - return num_residuals_; - } + int num_residuals() const { return num_residuals_; } protected: - std::vector<int32>* mutable_parameter_block_sizes() { + std::vector<int32_t>* mutable_parameter_block_sizes() { return ¶meter_block_sizes_; } - void set_num_residuals(int num_residuals) { - num_residuals_ = num_residuals; - } + void set_num_residuals(int num_residuals) { num_residuals_ = num_residuals; } private: // Cost function signature metadata: number of inputs & their sizes, // number of outputs (residuals). - std::vector<int32> parameter_block_sizes_; + std::vector<int32_t> parameter_block_sizes_; int num_residuals_; - CERES_DISALLOW_COPY_AND_ASSIGN(CostFunction); }; } // namespace ceres diff --git a/extern/ceres/include/ceres/cost_function_to_functor.h b/extern/ceres/include/ceres/cost_function_to_functor.h index d2dc94725e4..1beeb906063 100644 --- a/extern/ceres/include/ceres/cost_function_to_functor.h +++ b/extern/ceres/include/ceres/cost_function_to_functor.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -30,7 +30,7 @@ // // CostFunctionToFunctor is an adapter class that allows users to use // SizedCostFunction objects in templated functors which are to be used for -// automatic differentiation. This allows the user to seamlessly mix +// automatic differentiation. This allows the user to seamlessly mix // analytic, numeric and automatic differentiation. // // For example, let us assume that @@ -38,16 +38,15 @@ // class IntrinsicProjection : public SizedCostFunction<2, 5, 3> { // public: // IntrinsicProjection(const double* observation); -// virtual bool Evaluate(double const* const* parameters, -// double* residuals, -// double** jacobians) const; +// bool Evaluate(double const* const* parameters, +// double* residuals, +// double** jacobians) const override; // }; // // is a cost function that implements the projection of a point in its // local coordinate system onto its image plane and subtracts it from // the observed point projection. It can compute its residual and -// either via analytic or numerical differentiation can compute its -// jacobians. +// jacobians either via analytic or numerical differentiation. // // Now we would like to compose the action of this CostFunction with // the action of camera extrinsics, i.e., rotation and @@ -87,594 +86,83 @@ #ifndef CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_ #define CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_ +#include <cstdint> #include <numeric> +#include <tuple> +#include <utility> #include <vector> #include "ceres/cost_function.h" #include "ceres/dynamic_cost_function_to_functor.h" #include "ceres/internal/fixed_array.h" +#include "ceres/internal/parameter_dims.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" namespace ceres { -template <int kNumResiduals, - int N0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, - int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> +template <int kNumResiduals, int... Ns> class CostFunctionToFunctor { public: // Takes ownership of cost_function. explicit CostFunctionToFunctor(CostFunction* cost_function) : cost_functor_(cost_function) { - CHECK_NOTNULL(cost_function); + CHECK(cost_function != nullptr); CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC); - // This block breaks the 80 column rule to keep it somewhat readable. - CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || - ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || - ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT - << "Zero block cannot precede a non-zero block. Block sizes are " - << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " - << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " - << N8 << ", " << N9; - - const std::vector<int32>& parameter_block_sizes = + const std::vector<int32_t>& parameter_block_sizes = cost_function->parameter_block_sizes(); - const int num_parameter_blocks = - (N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) + - (N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0); + const int num_parameter_blocks = ParameterDims::kNumParameterBlocks; 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 - if (parameter_block_sizes.size() > 2) CHECK_EQ(N2, parameter_block_sizes[2]); // NOLINT - if (parameter_block_sizes.size() > 3) CHECK_EQ(N3, parameter_block_sizes[3]); // NOLINT - if (parameter_block_sizes.size() > 4) CHECK_EQ(N4, parameter_block_sizes[4]); // NOLINT - if (parameter_block_sizes.size() > 5) CHECK_EQ(N5, parameter_block_sizes[5]); // NOLINT - if (parameter_block_sizes.size() > 6) CHECK_EQ(N6, parameter_block_sizes[6]); // NOLINT - if (parameter_block_sizes.size() > 7) CHECK_EQ(N7, parameter_block_sizes[7]); // NOLINT - if (parameter_block_sizes.size() > 8) CHECK_EQ(N8, parameter_block_sizes[8]); // NOLINT - if (parameter_block_sizes.size() > 9) CHECK_EQ(N9, parameter_block_sizes[9]); // NOLINT - - CHECK_EQ(accumulate(parameter_block_sizes.begin(), - parameter_block_sizes.end(), 0), - N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9); - } - - bool operator()(const double* x0, double* residuals) const { - CHECK_NE(N0, 0); - CHECK_EQ(N1, 0); - CHECK_EQ(N2, 0); - CHECK_EQ(N3, 0); - CHECK_EQ(N4, 0); - CHECK_EQ(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - - return cost_functor_(&x0, residuals); - } - - bool operator()(const double* x0, - const double* x1, - double* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_EQ(N2, 0); - CHECK_EQ(N3, 0); - CHECK_EQ(N4, 0); - CHECK_EQ(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const double*> parameter_blocks(2); - parameter_blocks[0] = x0; - parameter_blocks[1] = x1; - return cost_functor_(parameter_blocks.get(), residuals); - } - - bool operator()(const double* x0, - const double* x1, - const double* x2, - double* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_EQ(N3, 0); - CHECK_EQ(N4, 0); - CHECK_EQ(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const double*> parameter_blocks(3); - parameter_blocks[0] = x0; - parameter_blocks[1] = x1; - parameter_blocks[2] = x2; - return cost_functor_(parameter_blocks.get(), residuals); - } + if (parameter_block_sizes.size() == num_parameter_blocks) { + for (int block = 0; block < num_parameter_blocks; ++block) { + CHECK_EQ(ParameterDims::GetDim(block), parameter_block_sizes[block]) + << "Parameter block size missmatch. The specified static parameter " + "block dimension does not match the one from the cost function."; + } + } - bool operator()(const double* x0, - const double* x1, - const double* x2, - const double* x3, - double* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_EQ(N4, 0); - CHECK_EQ(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const double*> parameter_blocks(4); - parameter_blocks[0] = x0; - parameter_blocks[1] = x1; - parameter_blocks[2] = x2; - parameter_blocks[3] = x3; - return cost_functor_(parameter_blocks.get(), residuals); + CHECK_EQ(accumulate( + parameter_block_sizes.begin(), parameter_block_sizes.end(), 0), + ParameterDims::kNumParameters); } - bool operator()(const double* x0, - const double* x1, - const double* x2, - const double* x3, - const double* x4, - double* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_EQ(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const double*> parameter_blocks(5); - parameter_blocks[0] = x0; - parameter_blocks[1] = x1; - parameter_blocks[2] = x2; - parameter_blocks[3] = x3; - parameter_blocks[4] = x4; - return cost_functor_(parameter_blocks.get(), residuals); - } + template <typename T, typename... Ts> + bool operator()(const T* p1, Ts*... ps) const { + // Add one because of residual block. + static_assert(sizeof...(Ts) + 1 == ParameterDims::kNumParameterBlocks + 1, + "Invalid number of parameter blocks specified."); - bool operator()(const double* x0, - const double* x1, - const double* x2, - const double* x3, - const double* x4, - const double* x5, - double* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_NE(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const double*> parameter_blocks(6); - parameter_blocks[0] = x0; - parameter_blocks[1] = x1; - parameter_blocks[2] = x2; - parameter_blocks[3] = x3; - parameter_blocks[4] = x4; - parameter_blocks[5] = x5; - return cost_functor_(parameter_blocks.get(), residuals); - } + auto params = std::make_tuple(p1, ps...); - bool operator()(const double* x0, - const double* x1, - const double* x2, - const double* x3, - const double* x4, - const double* x5, - const double* x6, - double* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_NE(N5, 0); - CHECK_NE(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const double*> parameter_blocks(7); - parameter_blocks[0] = x0; - parameter_blocks[1] = x1; - parameter_blocks[2] = x2; - parameter_blocks[3] = x3; - parameter_blocks[4] = x4; - parameter_blocks[5] = x5; - parameter_blocks[6] = x6; - return cost_functor_(parameter_blocks.get(), residuals); - } + // Extract residual pointer from params. The residual pointer is the + // last pointer. + constexpr int kResidualIndex = ParameterDims::kNumParameterBlocks; + T* residuals = std::get<kResidualIndex>(params); - bool operator()(const double* x0, - const double* x1, - const double* x2, - const double* x3, - const double* x4, - const double* x5, - const double* x6, - const double* x7, - double* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_NE(N5, 0); - CHECK_NE(N6, 0); - CHECK_NE(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const double*> parameter_blocks(8); - parameter_blocks[0] = x0; - parameter_blocks[1] = x1; - parameter_blocks[2] = x2; - parameter_blocks[3] = x3; - parameter_blocks[4] = x4; - parameter_blocks[5] = x5; - parameter_blocks[6] = x6; - parameter_blocks[7] = x7; - return cost_functor_(parameter_blocks.get(), residuals); - } - - bool operator()(const double* x0, - const double* x1, - const double* x2, - const double* x3, - const double* x4, - const double* x5, - const double* x6, - const double* x7, - const double* x8, - double* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_NE(N5, 0); - CHECK_NE(N6, 0); - CHECK_NE(N7, 0); - CHECK_NE(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const double*> parameter_blocks(9); - parameter_blocks[0] = x0; - parameter_blocks[1] = x1; - parameter_blocks[2] = x2; - parameter_blocks[3] = x3; - parameter_blocks[4] = x4; - parameter_blocks[5] = x5; - parameter_blocks[6] = x6; - parameter_blocks[7] = x7; - parameter_blocks[8] = x8; - return cost_functor_(parameter_blocks.get(), residuals); - } - - bool operator()(const double* x0, - const double* x1, - const double* x2, - const double* x3, - const double* x4, - const double* x5, - const double* x6, - const double* x7, - const double* x8, - const double* x9, - double* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_NE(N5, 0); - CHECK_NE(N6, 0); - CHECK_NE(N7, 0); - CHECK_NE(N8, 0); - CHECK_NE(N9, 0); - internal::FixedArray<const double*> parameter_blocks(10); - parameter_blocks[0] = x0; - parameter_blocks[1] = x1; - parameter_blocks[2] = x2; - parameter_blocks[3] = x3; - parameter_blocks[4] = x4; - parameter_blocks[5] = x5; - parameter_blocks[6] = x6; - parameter_blocks[7] = x7; - parameter_blocks[8] = x8; - parameter_blocks[9] = x9; - return cost_functor_(parameter_blocks.get(), residuals); - } + // Extract parameter block pointers from params. + using Indices = + std::make_integer_sequence<int, + ParameterDims::kNumParameterBlocks>; + std::array<const T*, ParameterDims::kNumParameterBlocks> parameter_blocks = + GetParameterPointers<T>(params, Indices()); - template <typename JetT> - bool operator()(const JetT* x0, JetT* residuals) const { - CHECK_NE(N0, 0); - CHECK_EQ(N1, 0); - CHECK_EQ(N2, 0); - CHECK_EQ(N3, 0); - CHECK_EQ(N4, 0); - CHECK_EQ(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - return cost_functor_(&x0, residuals); + return cost_functor_(parameter_blocks.data(), residuals); } - template <typename JetT> - bool operator()(const JetT* x0, - const JetT* x1, - JetT* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_EQ(N2, 0); - CHECK_EQ(N3, 0); - CHECK_EQ(N4, 0); - CHECK_EQ(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const JetT*> jets(2); - jets[0] = x0; - jets[1] = x1; - return cost_functor_(jets.get(), residuals); - } - - template <typename JetT> - bool operator()(const JetT* x0, - const JetT* x1, - const JetT* x2, - JetT* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_EQ(N3, 0); - CHECK_EQ(N4, 0); - CHECK_EQ(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const JetT*> jets(3); - jets[0] = x0; - jets[1] = x1; - jets[2] = x2; - return cost_functor_(jets.get(), residuals); - } - - template <typename JetT> - bool operator()(const JetT* x0, - const JetT* x1, - const JetT* x2, - const JetT* x3, - JetT* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_EQ(N4, 0); - CHECK_EQ(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const JetT*> jets(4); - jets[0] = x0; - jets[1] = x1; - jets[2] = x2; - jets[3] = x3; - return cost_functor_(jets.get(), residuals); - } - - template <typename JetT> - bool operator()(const JetT* x0, - const JetT* x1, - const JetT* x2, - const JetT* x3, - const JetT* x4, - JetT* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_EQ(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const JetT*> jets(5); - jets[0] = x0; - jets[1] = x1; - jets[2] = x2; - jets[3] = x3; - jets[4] = x4; - return cost_functor_(jets.get(), residuals); - } - - template <typename JetT> - bool operator()(const JetT* x0, - const JetT* x1, - const JetT* x2, - const JetT* x3, - const JetT* x4, - const JetT* x5, - JetT* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_NE(N5, 0); - CHECK_EQ(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const JetT*> jets(6); - jets[0] = x0; - jets[1] = x1; - jets[2] = x2; - jets[3] = x3; - jets[4] = x4; - jets[5] = x5; - return cost_functor_(jets.get(), residuals); - } - - template <typename JetT> - bool operator()(const JetT* x0, - const JetT* x1, - const JetT* x2, - const JetT* x3, - const JetT* x4, - const JetT* x5, - const JetT* x6, - JetT* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_NE(N5, 0); - CHECK_NE(N6, 0); - CHECK_EQ(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const JetT*> jets(7); - jets[0] = x0; - jets[1] = x1; - jets[2] = x2; - jets[3] = x3; - jets[4] = x4; - jets[5] = x5; - jets[6] = x6; - return cost_functor_(jets.get(), residuals); - } - - template <typename JetT> - bool operator()(const JetT* x0, - const JetT* x1, - const JetT* x2, - const JetT* x3, - const JetT* x4, - const JetT* x5, - const JetT* x6, - const JetT* x7, - JetT* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_NE(N5, 0); - CHECK_NE(N6, 0); - CHECK_NE(N7, 0); - CHECK_EQ(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const JetT*> jets(8); - jets[0] = x0; - jets[1] = x1; - jets[2] = x2; - jets[3] = x3; - jets[4] = x4; - jets[5] = x5; - jets[6] = x6; - jets[7] = x7; - return cost_functor_(jets.get(), residuals); - } - - template <typename JetT> - bool operator()(const JetT* x0, - const JetT* x1, - const JetT* x2, - const JetT* x3, - const JetT* x4, - const JetT* x5, - const JetT* x6, - const JetT* x7, - const JetT* x8, - JetT* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_NE(N5, 0); - CHECK_NE(N6, 0); - CHECK_NE(N7, 0); - CHECK_NE(N8, 0); - CHECK_EQ(N9, 0); - internal::FixedArray<const JetT*> jets(9); - jets[0] = x0; - jets[1] = x1; - jets[2] = x2; - jets[3] = x3; - jets[4] = x4; - jets[5] = x5; - jets[6] = x6; - jets[7] = x7; - jets[8] = x8; - return cost_functor_(jets.get(), residuals); - } + private: + using ParameterDims = internal::StaticParameterDims<Ns...>; - template <typename JetT> - bool operator()(const JetT* x0, - const JetT* x1, - const JetT* x2, - const JetT* x3, - const JetT* x4, - const JetT* x5, - const JetT* x6, - const JetT* x7, - const JetT* x8, - const JetT* x9, - JetT* residuals) const { - CHECK_NE(N0, 0); - CHECK_NE(N1, 0); - CHECK_NE(N2, 0); - CHECK_NE(N3, 0); - CHECK_NE(N4, 0); - CHECK_NE(N5, 0); - CHECK_NE(N6, 0); - CHECK_NE(N7, 0); - CHECK_NE(N8, 0); - CHECK_NE(N9, 0); - internal::FixedArray<const JetT*> jets(10); - jets[0] = x0; - jets[1] = x1; - jets[2] = x2; - jets[3] = x3; - jets[4] = x4; - jets[5] = x5; - jets[6] = x6; - jets[7] = x7; - jets[8] = x8; - jets[9] = x9; - return cost_functor_(jets.get(), residuals); + template <typename T, typename Tuple, int... Indices> + static std::array<const T*, ParameterDims::kNumParameterBlocks> + GetParameterPointers(const Tuple& paramPointers, + std::integer_sequence<int, Indices...>) { + return std::array<const T*, ParameterDims::kNumParameterBlocks>{ + {std::get<Indices>(paramPointers)...}}; } - private: DynamicCostFunctionToFunctor cost_functor_; }; diff --git a/extern/ceres/include/ceres/covariance.h b/extern/ceres/include/ceres/covariance.h index 930f96cf3ae..99825c425ad 100644 --- a/extern/ceres/include/ceres/covariance.h +++ b/extern/ceres/include/ceres/covariance.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,12 +31,13 @@ #ifndef CERES_PUBLIC_COVARIANCE_H_ #define CERES_PUBLIC_COVARIANCE_H_ +#include <memory> #include <utility> #include <vector> + +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" -#include "ceres/internal/disable_warnings.h" namespace ceres { @@ -60,7 +61,7 @@ class CovarianceImpl; // Background // ========== // One way to assess the quality of the solution returned by a -// non-linear least squares solve is to analyze the covariance of the +// non-linear least squares solver is to analyze the covariance of the // solution. // // Let us consider the non-linear regression problem @@ -158,7 +159,7 @@ class CovarianceImpl; // Gauge Invariance // ---------------- // In structure from motion (3D reconstruction) problems, the -// reconstruction is ambiguous upto a similarity transform. This is +// reconstruction is ambiguous up to a similarity transform. This is // known as a Gauge Ambiguity. Handling Gauges correctly requires the // use of SVD or custom inversion algorithms. For small problems the // user can use the dense algorithm. For more details see @@ -183,7 +184,7 @@ class CovarianceImpl; // Covariance::Options options; // Covariance covariance(options); // -// std::vector<std::pair<const double*, const double*> > covariance_blocks; +// std::vector<std::pair<const double*, const double*>> covariance_blocks; // covariance_blocks.push_back(make_pair(x, x)); // covariance_blocks.push_back(make_pair(y, y)); // covariance_blocks.push_back(make_pair(x, y)); @@ -200,19 +201,19 @@ class CovarianceImpl; class CERES_EXPORT Covariance { public: struct CERES_EXPORT Options { - Options() -#ifndef CERES_NO_SUITESPARSE - : algorithm_type(SUITE_SPARSE_QR), + // Sparse linear algebra library to use when a sparse matrix + // factorization is being used to compute the covariance matrix. + // + // Currently this only applies to SPARSE_QR. + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type = +#if !defined(CERES_NO_SUITESPARSE) + SUITE_SPARSE; #else - : algorithm_type(EIGEN_SPARSE_QR), + // Eigen's QR factorization is always available. + EIGEN_SPARSE; #endif - min_reciprocal_condition_number(1e-14), - null_space_rank(0), - num_threads(1), - apply_loss_function(true) { - } - // Ceres supports three different algorithms for covariance + // Ceres supports two different algorithms for covariance // estimation, which represent different tradeoffs in speed, // accuracy and reliability. // @@ -229,23 +230,20 @@ class CERES_EXPORT Covariance { // for small to moderate sized problems. It can handle // full-rank as well as rank deficient Jacobians. // - // 2. EIGEN_SPARSE_QR uses the sparse QR factorization algorithm - // in Eigen to compute the decomposition + // 2. SPARSE_QR uses the sparse QR factorization algorithm + // to compute the decomposition // // Q * R = J // // [J'J]^-1 = [R*R']^-1 // - // It is a moderately fast algorithm for sparse matrices. - // - // 3. SUITE_SPARSE_QR uses the SuiteSparseQR sparse QR - // factorization algorithm. It uses dense linear algebra and is - // multi threaded, so for large sparse sparse matrices it is - // significantly faster than EIGEN_SPARSE_QR. - // - // Neither EIGEN_SPARSE_QR not SUITE_SPARSE_QR are capable of - // computing the covariance if the Jacobian is rank deficient. - CovarianceAlgorithmType algorithm_type; + // SPARSE_QR is not capable of computing the covariance if the + // Jacobian is rank deficient. Depending on the value of + // Covariance::Options::sparse_linear_algebra_library_type, either + // Eigen's Sparse QR factorization algorithm will be used or + // SuiteSparse's high performance SuiteSparseQR algorithm will be + // used. + CovarianceAlgorithmType algorithm_type = SPARSE_QR; // If the Jacobian matrix is near singular, then inverting J'J // will result in unreliable results, e.g, if @@ -270,7 +268,7 @@ class CERES_EXPORT Covariance { // where min_sigma and max_sigma are the minimum and maxiumum // singular values of J respectively. // - // 2. SUITE_SPARSE_QR and EIGEN_SPARSE_QR + // 2. SPARSE_QR // // rank(J) < num_col(J) // @@ -278,7 +276,7 @@ class CERES_EXPORT Covariance { // sparse QR factorization algorithm. It is a fairly reliable // indication of rank deficiency. // - double min_reciprocal_condition_number; + double min_reciprocal_condition_number = 1e-14; // When using DENSE_SVD, the user has more control in dealing with // singular and near singular covariance matrices. @@ -313,9 +311,9 @@ class CERES_EXPORT Covariance { // // This option has no effect on the SUITE_SPARSE_QR and // EIGEN_SPARSE_QR algorithms. - int null_space_rank; + int null_space_rank = 0; - int num_threads; + int num_threads = 1; // Even though the residual blocks in the problem may contain loss // functions, setting apply_loss_function to false will turn off @@ -323,7 +321,7 @@ class CERES_EXPORT Covariance { // function and in turn its effect on the covariance. // // TODO(sameergaarwal): Expand this based on Jim's experiments. - bool apply_loss_function; + bool apply_loss_function = true; }; explicit Covariance(const Options& options); @@ -352,10 +350,9 @@ class CERES_EXPORT Covariance { // covariance computation. Please see the documentation for // Covariance::Options for more on the conditions under which this // function returns false. - bool Compute( - const std::vector<std::pair<const double*, - const double*> >& covariance_blocks, - Problem* problem); + bool Compute(const std::vector<std::pair<const double*, const double*>>& + covariance_blocks, + Problem* problem); // Compute a part of the covariance matrix. // @@ -428,8 +425,8 @@ class CERES_EXPORT Covariance { // 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); + bool GetCovarianceMatrix(const std::vector<const double*>& parameter_blocks, + double* covariance_matrix) const; // Return the covariance matrix corresponding to parameter_blocks // in the tangent space if a local parameterization is associated @@ -448,10 +445,10 @@ class CERES_EXPORT Covariance { // blocks. The covariance matrix will be a row-major matrix. bool GetCovarianceMatrixInTangentSpace( const std::vector<const double*>& parameter_blocks, - double* covariance_matrix); + double* covariance_matrix) const; private: - internal::scoped_ptr<internal::CovarianceImpl> impl_; + std::unique_ptr<internal::CovarianceImpl> impl_; }; } // namespace ceres diff --git a/extern/ceres/include/ceres/crs_matrix.h b/extern/ceres/include/ceres/crs_matrix.h index 23687c4670e..bc618fa0905 100644 --- a/extern/ceres/include/ceres/crs_matrix.h +++ b/extern/ceres/include/ceres/crs_matrix.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -32,8 +32,9 @@ #define CERES_PUBLIC_CRS_MATRIX_H_ #include <vector> -#include "ceres/internal/port.h" + #include "ceres/internal/disable_warnings.h" +#include "ceres/internal/port.h" namespace ceres { diff --git a/extern/ceres/include/ceres/cubic_interpolation.h b/extern/ceres/include/ceres/cubic_interpolation.h new file mode 100644 index 00000000000..9b9ea4a942c --- /dev/null +++ b/extern/ceres/include/ceres/cubic_interpolation.h @@ -0,0 +1,436 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_CUBIC_INTERPOLATION_H_ +#define CERES_PUBLIC_CUBIC_INTERPOLATION_H_ + +#include "Eigen/Core" +#include "ceres/internal/port.h" +#include "glog/logging.h" + +namespace ceres { + +// Given samples from a function sampled at four equally spaced points, +// +// p0 = f(-1) +// p1 = f(0) +// p2 = f(1) +// p3 = f(2) +// +// Evaluate the cubic Hermite spline (also known as the Catmull-Rom +// spline) at a point x that lies in the interval [0, 1]. +// +// This is also the interpolation kernel (for the case of a = 0.5) as +// proposed by R. Keys, in: +// +// "Cubic convolution interpolation for digital image processing". +// IEEE Transactions on Acoustics, Speech, and Signal Processing +// 29 (6): 1153-1160. +// +// For more details see +// +// http://en.wikipedia.org/wiki/Cubic_Hermite_spline +// http://en.wikipedia.org/wiki/Bicubic_interpolation +// +// f if not NULL will contain the interpolated function values. +// dfdx if not NULL will contain the interpolated derivative values. +template <int kDataDimension> +void CubicHermiteSpline(const Eigen::Matrix<double, kDataDimension, 1>& p0, + const Eigen::Matrix<double, kDataDimension, 1>& p1, + const Eigen::Matrix<double, kDataDimension, 1>& p2, + const Eigen::Matrix<double, kDataDimension, 1>& p3, + const double x, + double* f, + double* dfdx) { + typedef Eigen::Matrix<double, kDataDimension, 1> VType; + const VType a = 0.5 * (-p0 + 3.0 * p1 - 3.0 * p2 + p3); + const VType b = 0.5 * (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3); + const VType c = 0.5 * (-p0 + p2); + const VType d = p1; + + // Use Horner's rule to evaluate the function value and its + // derivative. + + // f = ax^3 + bx^2 + cx + d + if (f != NULL) { + Eigen::Map<VType>(f, kDataDimension) = d + x * (c + x * (b + x * a)); + } + + // dfdx = 3ax^2 + 2bx + c + if (dfdx != NULL) { + Eigen::Map<VType>(dfdx, kDataDimension) = c + x * (2.0 * b + 3.0 * a * x); + } +} + +// Given as input an infinite one dimensional grid, which provides the +// following interface. +// +// class Grid { +// public: +// enum { DATA_DIMENSION = 2; }; +// void GetValue(int n, double* f) const; +// }; +// +// Here, GetValue gives the value of a function f (possibly vector +// valued) for any integer n. +// +// The enum DATA_DIMENSION indicates the dimensionality of the +// function being interpolated. For example if you are interpolating +// rotations in axis-angle format over time, then DATA_DIMENSION = 3. +// +// CubicInterpolator uses cubic Hermite splines to produce a smooth +// approximation to it that can be used to evaluate the f(x) and f'(x) +// at any point on the real number line. +// +// For more details on cubic interpolation see +// +// http://en.wikipedia.org/wiki/Cubic_Hermite_spline +// +// Example usage: +// +// const double data[] = {1.0, 2.0, 5.0, 6.0}; +// Grid1D<double, 1> grid(data, 0, 4); +// CubicInterpolator<Grid1D<double, 1>> interpolator(grid); +// double f, dfdx; +// interpolator.Evaluator(1.5, &f, &dfdx); +template <typename Grid> +class CubicInterpolator { + public: + explicit CubicInterpolator(const Grid& grid) : grid_(grid) { + // The + casts the enum into an int before doing the + // comparison. It is needed to prevent + // "-Wunnamed-type-template-args" related errors. + CHECK_GE(+Grid::DATA_DIMENSION, 1); + } + + void Evaluate(double x, double* f, double* dfdx) const { + const int n = std::floor(x); + Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3; + grid_.GetValue(n - 1, p0.data()); + grid_.GetValue(n, p1.data()); + grid_.GetValue(n + 1, p2.data()); + grid_.GetValue(n + 2, p3.data()); + CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, x - n, f, dfdx); + } + + // The following two Evaluate overloads are needed for interfacing + // with automatic differentiation. The first is for when a scalar + // evaluation is done, and the second one is for when Jets are used. + void Evaluate(const double& x, double* f) const { Evaluate(x, f, NULL); } + + template <typename JetT> + void Evaluate(const JetT& x, JetT* f) const { + double fx[Grid::DATA_DIMENSION], dfdx[Grid::DATA_DIMENSION]; + Evaluate(x.a, fx, dfdx); + for (int i = 0; i < Grid::DATA_DIMENSION; ++i) { + f[i].a = fx[i]; + f[i].v = dfdx[i] * x.v; + } + } + + private: + const Grid& grid_; +}; + +// An object that implements an infinite one dimensional grid needed +// by the CubicInterpolator where the source of the function values is +// an array of type T on the interval +// +// [begin, ..., end - 1] +// +// Since the input array is finite and the grid is infinite, values +// outside this interval needs to be computed. Grid1D uses the value +// from the nearest edge. +// +// The function being provided can be vector valued, in which case +// kDataDimension > 1. The dimensional slices of the function maybe +// interleaved, or they maybe stacked, i.e, if the function has +// kDataDimension = 2, if kInterleaved = true, then it is stored as +// +// f01, f02, f11, f12 .... +// +// and if kInterleaved = false, then it is stored as +// +// f01, f11, .. fn1, f02, f12, .. , fn2 +// +template <typename T, int kDataDimension = 1, bool kInterleaved = true> +struct Grid1D { + public: + enum { DATA_DIMENSION = kDataDimension }; + + Grid1D(const T* data, const int begin, const int end) + : data_(data), begin_(begin), end_(end), num_values_(end - begin) { + CHECK_LT(begin, end); + } + + EIGEN_STRONG_INLINE void GetValue(const int n, double* f) const { + const int idx = std::min(std::max(begin_, n), end_ - 1) - begin_; + if (kInterleaved) { + for (int i = 0; i < kDataDimension; ++i) { + f[i] = static_cast<double>(data_[kDataDimension * idx + i]); + } + } else { + for (int i = 0; i < kDataDimension; ++i) { + f[i] = static_cast<double>(data_[i * num_values_ + idx]); + } + } + } + + private: + const T* data_; + const int begin_; + const int end_; + const int num_values_; +}; + +// Given as input an infinite two dimensional grid like object, which +// provides the following interface: +// +// struct Grid { +// enum { DATA_DIMENSION = 1 }; +// void GetValue(int row, int col, double* f) const; +// }; +// +// Where, GetValue gives us the value of a function f (possibly vector +// valued) for any pairs of integers (row, col), and the enum +// DATA_DIMENSION indicates the dimensionality of the function being +// interpolated. For example if you are interpolating a color image +// with three channels (Red, Green & Blue), then DATA_DIMENSION = 3. +// +// BiCubicInterpolator uses the cubic convolution interpolation +// algorithm of R. Keys, to produce a smooth approximation to it that +// can be used to evaluate the f(r,c), df(r, c)/dr and df(r,c)/dc at +// any point in the real plane. +// +// For more details on the algorithm used here see: +// +// "Cubic convolution interpolation for digital image processing". +// Robert G. Keys, IEEE Trans. on Acoustics, Speech, and Signal +// Processing 29 (6): 1153-1160, 1981. +// +// http://en.wikipedia.org/wiki/Cubic_Hermite_spline +// http://en.wikipedia.org/wiki/Bicubic_interpolation +// +// Example usage: +// +// const double data[] = {1.0, 3.0, -1.0, 4.0, +// 3.6, 2.1, 4.2, 2.0, +// 2.0, 1.0, 3.1, 5.2}; +// Grid2D<double, 1> grid(data, 3, 4); +// BiCubicInterpolator<Grid2D<double, 1>> interpolator(grid); +// double f, dfdr, dfdc; +// interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc); + +template <typename Grid> +class BiCubicInterpolator { + public: + explicit BiCubicInterpolator(const Grid& grid) : grid_(grid) { + // The + casts the enum into an int before doing the + // comparison. It is needed to prevent + // "-Wunnamed-type-template-args" related errors. + CHECK_GE(+Grid::DATA_DIMENSION, 1); + } + + // Evaluate the interpolated function value and/or its + // derivative. Uses the nearest point on the grid boundary if r or + // c is out of bounds. + void Evaluate( + double r, double c, double* f, double* dfdr, double* dfdc) const { + // BiCubic interpolation requires 16 values around the point being + // evaluated. We will use pij, to indicate the elements of the + // 4x4 grid of values. + // + // col + // p00 p01 p02 p03 + // row p10 p11 p12 p13 + // p20 p21 p22 p23 + // p30 p31 p32 p33 + // + // The point (r,c) being evaluated is assumed to lie in the square + // defined by p11, p12, p22 and p21. + + const int row = std::floor(r); + const int col = std::floor(c); + + Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3; + + // Interpolate along each of the four rows, evaluating the function + // value and the horizontal derivative in each row. + Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> f0, f1, f2, f3; + Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> df0dc, df1dc, df2dc, df3dc; + + grid_.GetValue(row - 1, col - 1, p0.data()); + grid_.GetValue(row - 1, col, p1.data()); + grid_.GetValue(row - 1, col + 1, p2.data()); + grid_.GetValue(row - 1, col + 2, p3.data()); + CubicHermiteSpline<Grid::DATA_DIMENSION>( + p0, p1, p2, p3, c - col, f0.data(), df0dc.data()); + + grid_.GetValue(row, col - 1, p0.data()); + grid_.GetValue(row, col, p1.data()); + grid_.GetValue(row, col + 1, p2.data()); + grid_.GetValue(row, col + 2, p3.data()); + CubicHermiteSpline<Grid::DATA_DIMENSION>( + p0, p1, p2, p3, c - col, f1.data(), df1dc.data()); + + grid_.GetValue(row + 1, col - 1, p0.data()); + grid_.GetValue(row + 1, col, p1.data()); + grid_.GetValue(row + 1, col + 1, p2.data()); + grid_.GetValue(row + 1, col + 2, p3.data()); + CubicHermiteSpline<Grid::DATA_DIMENSION>( + p0, p1, p2, p3, c - col, f2.data(), df2dc.data()); + + grid_.GetValue(row + 2, col - 1, p0.data()); + grid_.GetValue(row + 2, col, p1.data()); + grid_.GetValue(row + 2, col + 1, p2.data()); + grid_.GetValue(row + 2, col + 2, p3.data()); + CubicHermiteSpline<Grid::DATA_DIMENSION>( + p0, p1, p2, p3, c - col, f3.data(), df3dc.data()); + + // Interpolate vertically the interpolated value from each row and + // compute the derivative along the columns. + CubicHermiteSpline<Grid::DATA_DIMENSION>(f0, f1, f2, f3, r - row, f, dfdr); + if (dfdc != NULL) { + // Interpolate vertically the derivative along the columns. + CubicHermiteSpline<Grid::DATA_DIMENSION>( + df0dc, df1dc, df2dc, df3dc, r - row, dfdc, NULL); + } + } + + // The following two Evaluate overloads are needed for interfacing + // with automatic differentiation. The first is for when a scalar + // evaluation is done, and the second one is for when Jets are used. + void Evaluate(const double& r, const double& c, double* f) const { + Evaluate(r, c, f, NULL, NULL); + } + + template <typename JetT> + void Evaluate(const JetT& r, const JetT& c, JetT* f) const { + double frc[Grid::DATA_DIMENSION]; + double dfdr[Grid::DATA_DIMENSION]; + double dfdc[Grid::DATA_DIMENSION]; + Evaluate(r.a, c.a, frc, dfdr, dfdc); + for (int i = 0; i < Grid::DATA_DIMENSION; ++i) { + f[i].a = frc[i]; + f[i].v = dfdr[i] * r.v + dfdc[i] * c.v; + } + } + + private: + const Grid& grid_; +}; + +// An object that implements an infinite two dimensional grid needed +// by the BiCubicInterpolator where the source of the function values +// is an grid of type T on the grid +// +// [(row_start, col_start), ..., (row_start, col_end - 1)] +// [ ... ] +// [(row_end - 1, col_start), ..., (row_end - 1, col_end - 1)] +// +// Since the input grid is finite and the grid is infinite, values +// outside this interval needs to be computed. Grid2D uses the value +// from the nearest edge. +// +// The function being provided can be vector valued, in which case +// kDataDimension > 1. The data maybe stored in row or column major +// format and the various dimensional slices of the function maybe +// interleaved, or they maybe stacked, i.e, if the function has +// kDataDimension = 2, is stored in row-major format and if +// kInterleaved = true, then it is stored as +// +// f001, f002, f011, f012, ... +// +// A commonly occuring example are color images (RGB) where the three +// channels are stored interleaved. +// +// If kInterleaved = false, then it is stored as +// +// f001, f011, ..., fnm1, f002, f012, ... +template <typename T, + int kDataDimension = 1, + bool kRowMajor = true, + bool kInterleaved = true> +struct Grid2D { + public: + enum { DATA_DIMENSION = kDataDimension }; + + Grid2D(const T* data, + const int row_begin, + const int row_end, + const int col_begin, + const int col_end) + : data_(data), + row_begin_(row_begin), + row_end_(row_end), + col_begin_(col_begin), + col_end_(col_end), + num_rows_(row_end - row_begin), + num_cols_(col_end - col_begin), + num_values_(num_rows_ * num_cols_) { + CHECK_GE(kDataDimension, 1); + CHECK_LT(row_begin, row_end); + CHECK_LT(col_begin, col_end); + } + + EIGEN_STRONG_INLINE void GetValue(const int r, const int c, double* f) const { + const int row_idx = + std::min(std::max(row_begin_, r), row_end_ - 1) - row_begin_; + const int col_idx = + std::min(std::max(col_begin_, c), col_end_ - 1) - col_begin_; + + const int n = (kRowMajor) ? num_cols_ * row_idx + col_idx + : num_rows_ * col_idx + row_idx; + + if (kInterleaved) { + for (int i = 0; i < kDataDimension; ++i) { + f[i] = static_cast<double>(data_[kDataDimension * n + i]); + } + } else { + for (int i = 0; i < kDataDimension; ++i) { + f[i] = static_cast<double>(data_[i * num_values_ + n]); + } + } + } + + private: + const T* data_; + const int row_begin_; + const int row_end_; + const int col_begin_; + const int col_end_; + const int num_rows_; + const int num_cols_; + const int num_values_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_CUBIC_INTERPOLATOR_H_ diff --git a/extern/ceres/include/ceres/dynamic_autodiff_cost_function.h b/extern/ceres/include/ceres/dynamic_autodiff_cost_function.h index e6d26111f18..7b75150b5ce 100644 --- a/extern/ceres/include/ceres/dynamic_autodiff_cost_function.h +++ b/extern/ceres/include/ceres/dynamic_autodiff_cost_function.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -28,7 +28,22 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) // mierle@gmail.com (Keir Mierle) -// + +#ifndef CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ + +#include <cmath> +#include <memory> +#include <numeric> +#include <vector> + +#include "ceres/dynamic_cost_function.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/jet.h" +#include "glog/logging.h" + +namespace ceres { + // This autodiff implementation differs from the one found in // autodiff_cost_function.h by supporting autodiff on cost functions // with variable numbers of parameters with variable sizes. With the @@ -43,7 +58,7 @@ // bool operator()(T const* const* parameters, T* residuals) const { // // Use parameters[i] to access the i'th parameter block. // } -// } +// }; // // Since the sizing of the parameters is done at runtime, you must // also specify the sizes after creating the dynamic autodiff cost @@ -60,40 +75,17 @@ // default, controlled by the Stride template parameter) with each // pass. There is a tradeoff with the size of the passes; you may want // to experiment with the stride. - -#ifndef CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ -#define CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ - -#include <cmath> -#include <numeric> -#include <vector> - -#include "ceres/cost_function.h" -#include "ceres/internal/scoped_ptr.h" -#include "ceres/jet.h" -#include "glog/logging.h" - -namespace ceres { - template <typename CostFunctor, int Stride = 4> -class DynamicAutoDiffCostFunction : public CostFunction { +class DynamicAutoDiffCostFunction : public DynamicCostFunction { public: explicit DynamicAutoDiffCostFunction(CostFunctor* functor) - : functor_(functor) {} + : functor_(functor) {} virtual ~DynamicAutoDiffCostFunction() {} - void AddParameterBlock(int size) { - mutable_parameter_block_sizes()->push_back(size); - } - - void SetNumResiduals(int num_residuals) { - set_num_residuals(num_residuals); - } - - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override { CHECK_GT(num_residuals(), 0) << "You must call DynamicAutoDiffCostFunction::SetNumResiduals() " << "before DynamicAutoDiffCostFunction::Evaluate()."; @@ -112,20 +104,23 @@ class DynamicAutoDiffCostFunction : public CostFunction { // depends on. // // To work around this issue, the solution here is to evaluate the - // jacobians in a series of passes, each one computing Stripe * + // jacobians in a series of passes, each one computing Stride * // num_residuals() derivatives. This is done with small, fixed-size jets. - const int num_parameter_blocks = parameter_block_sizes().size(); - const int num_parameters = std::accumulate(parameter_block_sizes().begin(), - parameter_block_sizes().end(), - 0); + const int num_parameter_blocks = + static_cast<int>(parameter_block_sizes().size()); + const int num_parameters = std::accumulate( + parameter_block_sizes().begin(), parameter_block_sizes().end(), 0); // Allocate scratch space for the strided evaluation. - std::vector<Jet<double, Stride> > input_jets(num_parameters); - std::vector<Jet<double, Stride> > output_jets(num_residuals()); + using JetT = Jet<double, Stride>; + internal::FixedArray<JetT, (256 * 7) / sizeof(JetT)> input_jets( + num_parameters); + internal::FixedArray<JetT, (256 * 7) / sizeof(JetT)> output_jets( + num_residuals()); // Make the parameter pack that is sent to the functor (reused). - std::vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks, - static_cast<Jet<double, Stride>* >(NULL)); + internal::FixedArray<Jet<double, Stride>*> jet_parameters( + num_parameter_blocks, nullptr); int num_active_parameters = 0; // To handle constant parameters between non-constant parameter blocks, the @@ -172,8 +167,8 @@ class DynamicAutoDiffCostFunction : public CostFunction { // Evaluate all of the strides. Each stride is a chunk of the derivative to // evaluate, typically some size proportional to the size of the SIMD // registers of the CPU. - int num_strides = static_cast<int>(ceil(num_active_parameters / - static_cast<float>(Stride))); + int num_strides = static_cast<int>( + ceil(num_active_parameters / static_cast<float>(Stride))); int current_derivative_section = 0; int current_derivative_section_cursor = 0; @@ -183,7 +178,7 @@ class DynamicAutoDiffCostFunction : public CostFunction { // non-constant #Stride parameters. const int initial_derivative_section = current_derivative_section; const int initial_derivative_section_cursor = - current_derivative_section_cursor; + current_derivative_section_cursor; int active_parameter_count = 0; parameter_cursor = 0; @@ -193,9 +188,9 @@ class DynamicAutoDiffCostFunction : public CostFunction { ++j, parameter_cursor++) { input_jets[parameter_cursor].v.setZero(); if (active_parameter_count < Stride && - parameter_cursor >= ( - start_derivative_section[current_derivative_section] + - current_derivative_section_cursor)) { + parameter_cursor >= + (start_derivative_section[current_derivative_section] + + current_derivative_section_cursor)) { if (jacobians[i] != NULL) { input_jets[parameter_cursor].v[active_parameter_count] = 1.0; ++active_parameter_count; @@ -222,9 +217,9 @@ class DynamicAutoDiffCostFunction : public CostFunction { for (int j = 0; j < parameter_block_sizes()[i]; ++j, parameter_cursor++) { if (active_parameter_count < Stride && - parameter_cursor >= ( - start_derivative_section[current_derivative_section] + - current_derivative_section_cursor)) { + parameter_cursor >= + (start_derivative_section[current_derivative_section] + + current_derivative_section_cursor)) { if (jacobians[i] != NULL) { for (int k = 0; k < num_residuals(); ++k) { jacobians[i][k * parameter_block_sizes()[i] + j] = @@ -252,7 +247,7 @@ class DynamicAutoDiffCostFunction : public CostFunction { } private: - internal::scoped_ptr<CostFunctor> functor_; + std::unique_ptr<CostFunctor> functor_; }; } // namespace ceres diff --git a/extern/ceres/include/ceres/fpclassify.h b/extern/ceres/include/ceres/dynamic_cost_function.h index bc2dc90026c..6e8a076ecd0 100644 --- a/extern/ceres/include/ceres/fpclassify.h +++ b/extern/ceres/include/ceres/dynamic_cost_function.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -26,45 +26,31 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. // -// Author: keir@google.com (Keir Mierle) -// -// Portable floating point classification. The names are picked such that they -// do not collide with macros. For example, "isnan" in C99 is a macro and hence -// does not respect namespaces. -// -// TODO(keir): Finish porting! - -#ifndef CERES_PUBLIC_FPCLASSIFY_H_ -#define CERES_PUBLIC_FPCLASSIFY_H_ +// Author: sameeragarwal@google.com (Sameer Agarwal) -#if defined(_MSC_VER) -#include <float.h> -#endif +#ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_ +#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_ -#include <limits> +#include "ceres/cost_function.h" namespace ceres { -#if defined(_MSC_VER) - -inline bool IsFinite (double x) { return _finite(x) != 0; } -inline bool IsInfinite(double x) { return _finite(x) == 0 && _isnan(x) == 0; } -inline bool IsNaN (double x) { return _isnan(x) != 0; } -inline bool IsNormal (double x) { // NOLINT - const int classification = _fpclass(x); - return (classification == _FPCLASS_NN || classification == _FPCLASS_PN); -} - -# else +// A common base class for DynamicAutoDiffCostFunction and +// DynamicNumericDiffCostFunction which depend on methods that can add +// parameter blocks and set the number of residuals at run time. +class CERES_EXPORT DynamicCostFunction : public CostFunction { + public: + ~DynamicCostFunction() {} -// These definitions are for the normal Unix suspects. -inline bool IsFinite (double x) { return std::isfinite(x); } -inline bool IsInfinite(double x) { return std::isinf(x); } -inline bool IsNaN (double x) { return std::isnan(x); } -inline bool IsNormal (double x) { return std::isnormal(x); } + virtual void AddParameterBlock(int size) { + mutable_parameter_block_sizes()->push_back(size); + } -#endif + virtual void SetNumResiduals(int num_residuals) { + set_num_residuals(num_residuals); + } +}; } // namespace ceres -#endif // CERES_PUBLIC_FPCLASSIFY_H_ +#endif // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/dynamic_cost_function_to_functor.h b/extern/ceres/include/ceres/dynamic_cost_function_to_functor.h index 9339a503ea0..8d174d8ecc2 100644 --- a/extern/ceres/include/ceres/dynamic_cost_function_to_functor.h +++ b/extern/ceres/include/ceres/dynamic_cost_function_to_functor.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -28,7 +28,20 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) // dgossow@google.com (David Gossow) -// + +#ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_ +#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_ + +#include <memory> +#include <numeric> +#include <vector> + +#include "ceres/dynamic_cost_function.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/port.h" + +namespace ceres { + // DynamicCostFunctionToFunctor allows users to use CostFunction // objects in templated functors which are to be used for automatic // differentiation. It works similar to CostFunctionToFunctor, with the @@ -40,9 +53,9 @@ // class IntrinsicProjection : public CostFunction { // public: // IntrinsicProjection(const double* observation); -// virtual bool Evaluate(double const* const* parameters, -// double* residuals, -// double** jacobians) const; +// bool Evaluate(double const* const* parameters, +// double* residuals, +// double** jacobians) const override; // }; // // is a cost function that implements the projection of a point in its @@ -87,26 +100,12 @@ // private: // DynamicCostFunctionToFunctor intrinsic_projection_; // }; - -#ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_ -#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_ - -#include <numeric> -#include <vector> - -#include "ceres/cost_function.h" -#include "ceres/internal/fixed_array.h" -#include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" - -namespace ceres { - class DynamicCostFunctionToFunctor { public: // Takes ownership of cost_function. explicit DynamicCostFunctionToFunctor(CostFunction* cost_function) : cost_function_(cost_function) { - CHECK_NOTNULL(cost_function); + CHECK(cost_function != nullptr); } bool operator()(double const* const* parameters, double* residuals) const { @@ -115,12 +114,13 @@ class DynamicCostFunctionToFunctor { template <typename JetT> bool operator()(JetT const* const* inputs, JetT* output) const { - const std::vector<int32>& parameter_block_sizes = + const std::vector<int32_t>& parameter_block_sizes = cost_function_->parameter_block_sizes(); - const int num_parameter_blocks = parameter_block_sizes.size(); + const int num_parameter_blocks = + static_cast<int>(parameter_block_sizes.size()); const int num_residuals = cost_function_->num_residuals(); - const int num_parameters = std::accumulate(parameter_block_sizes.begin(), - parameter_block_sizes.end(), 0); + const int num_parameters = std::accumulate( + parameter_block_sizes.begin(), parameter_block_sizes.end(), 0); internal::FixedArray<double> parameters(num_parameters); internal::FixedArray<double*> parameter_blocks(num_parameter_blocks); @@ -130,8 +130,8 @@ class DynamicCostFunctionToFunctor { // Build a set of arrays to get the residuals and jacobians from // the CostFunction wrapped by this functor. - double* parameter_ptr = parameters.get(); - double* jacobian_ptr = jacobians.get(); + double* parameter_ptr = parameters.data(); + double* jacobian_ptr = jacobians.data(); for (int i = 0; i < num_parameter_blocks; ++i) { parameter_blocks[i] = parameter_ptr; jacobian_blocks[i] = jacobian_ptr; @@ -141,9 +141,9 @@ class DynamicCostFunctionToFunctor { jacobian_ptr += num_residuals * parameter_block_sizes[i]; } - if (!cost_function_->Evaluate(parameter_blocks.get(), - residuals.get(), - jacobian_blocks.get())) { + if (!cost_function_->Evaluate(parameter_blocks.data(), + residuals.data(), + jacobian_blocks.data())) { return false; } @@ -170,7 +170,7 @@ class DynamicCostFunctionToFunctor { output[i].v.setZero(); for (int j = 0; j < num_parameter_blocks; ++j) { - const int32 block_size = parameter_block_sizes[j]; + const int32_t block_size = parameter_block_sizes[j]; for (int k = 0; k < parameter_block_sizes[j]; ++k) { output[i].v += jacobian_blocks[j][i * block_size + k] * inputs[j][k].v; @@ -182,7 +182,7 @@ class DynamicCostFunctionToFunctor { } private: - internal::scoped_ptr<CostFunction> cost_function_; + std::unique_ptr<CostFunction> cost_function_; }; } // namespace ceres 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 5770946a115..119b3f85e8e 100644 --- a/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h +++ b/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -30,7 +30,24 @@ // sameeragarwal@google.com (Sameer Agarwal) // thadh@gmail.com (Thad Hughes) // tbennun@gmail.com (Tal Ben-Nun) -// + +#ifndef CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ + +#include <cmath> +#include <memory> +#include <numeric> +#include <vector> + +#include "ceres/dynamic_cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/numeric_diff.h" +#include "ceres/internal/parameter_dims.h" +#include "ceres/numeric_diff_options.h" +#include "glog/logging.h" + +namespace ceres { + // This numeric diff implementation differs from the one found in // numeric_diff_cost_function.h by supporting numericdiff on cost // functions with variable numbers of parameters with variable @@ -42,7 +59,9 @@ // numeric diff; the expected interface for the cost functors is: // // struct MyCostFunctor { -// bool operator()(double const* const* parameters, double* residuals) const { +// bool operator()(double const* +// const* parameters, +// double* residuals) const { // // Use parameters[i] to access the i'th parameter block. // } // } @@ -56,34 +75,14 @@ // cost_function.AddParameterBlock(5); // cost_function.AddParameterBlock(10); // cost_function.SetNumResiduals(21); - -#ifndef CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ -#define CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ - -#include <cmath> -#include <numeric> -#include <vector> - -#include "ceres/cost_function.h" -#include "ceres/internal/scoped_ptr.h" -#include "ceres/internal/eigen.h" -#include "ceres/internal/numeric_diff.h" -#include "ceres/numeric_diff_options.h" -#include "glog/logging.h" - -namespace ceres { - template <typename CostFunctor, NumericDiffMethodType method = CENTRAL> -class DynamicNumericDiffCostFunction : public CostFunction { +class DynamicNumericDiffCostFunction : public DynamicCostFunction { public: explicit DynamicNumericDiffCostFunction( const CostFunctor* functor, Ownership ownership = TAKE_OWNERSHIP, const NumericDiffOptions& options = NumericDiffOptions()) - : functor_(functor), - ownership_(ownership), - options_(options) { - } + : functor_(functor), ownership_(ownership), options_(options) {} virtual ~DynamicNumericDiffCostFunction() { if (ownership_ != TAKE_OWNERSHIP) { @@ -91,28 +90,22 @@ class DynamicNumericDiffCostFunction : public CostFunction { } } - void AddParameterBlock(int size) { - mutable_parameter_block_sizes()->push_back(size); - } - - void SetNumResiduals(int num_residuals) { - set_num_residuals(num_residuals); - } - - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override { using internal::NumericDiff; CHECK_GT(num_residuals(), 0) << "You must call DynamicNumericDiffCostFunction::SetNumResiduals() " << "before DynamicNumericDiffCostFunction::Evaluate()."; - const std::vector<int32>& block_sizes = parameter_block_sizes(); + const std::vector<int32_t>& block_sizes = parameter_block_sizes(); CHECK(!block_sizes.empty()) << "You must call DynamicNumericDiffCostFunction::AddParameterBlock() " << "before DynamicNumericDiffCostFunction::Evaluate()."; - const bool status = EvaluateCostFunctor(parameters, residuals); + const bool status = + internal::VariadicEvaluate<internal::DynamicParameterDims>( + *functor_.get(), parameters, residuals); if (jacobians == NULL || !status) { return status; } @@ -123,8 +116,8 @@ class DynamicNumericDiffCostFunction : public CostFunction { std::vector<double*> parameters_references_copy(block_sizes.size()); parameters_references_copy[0] = ¶meters_copy[0]; for (size_t block = 1; block < block_sizes.size(); ++block) { - parameters_references_copy[block] = parameters_references_copy[block - 1] - + block_sizes[block - 1]; + parameters_references_copy[block] = + parameters_references_copy[block - 1] + block_sizes[block - 1]; } // Copy the parameters into the local temp space. @@ -136,18 +129,20 @@ class DynamicNumericDiffCostFunction : public CostFunction { for (size_t block = 0; block < block_sizes.size(); ++block) { if (jacobians[block] != NULL && - !NumericDiff<CostFunctor, method, DYNAMIC, - DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, - DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, - DYNAMIC, DYNAMIC>::EvaluateJacobianForParameterBlock( - functor_.get(), - residuals, - options_, - this->num_residuals(), - block, - block_sizes[block], - ¶meters_references_copy[0], - jacobians[block])) { + !NumericDiff<CostFunctor, + method, + ceres::DYNAMIC, + internal::DynamicParameterDims, + ceres::DYNAMIC, + ceres::DYNAMIC>:: + EvaluateJacobianForParameterBlock(functor_.get(), + residuals, + options_, + this->num_residuals(), + block, + block_sizes[block], + ¶meters_references_copy[0], + jacobians[block])) { return false; } } @@ -155,31 +150,7 @@ class DynamicNumericDiffCostFunction : public CostFunction { } private: - bool EvaluateCostFunctor(double const* const* parameters, - double* residuals) const { - return EvaluateCostFunctorImpl(functor_.get(), - parameters, - residuals, - functor_.get()); - } - - // Helper templates to allow evaluation of a functor or a - // CostFunction. - bool EvaluateCostFunctorImpl(const CostFunctor* functor, - double const* const* parameters, - double* residuals, - const void* /* NOT USED */) const { - return (*functor)(parameters, residuals); - } - - bool EvaluateCostFunctorImpl(const CostFunctor* functor, - double const* const* parameters, - double* residuals, - const CostFunction* /* NOT USED */) const { - return functor->Evaluate(parameters, residuals, NULL); - } - - internal::scoped_ptr<const CostFunctor> functor_; + std::unique_ptr<const CostFunctor> functor_; Ownership ownership_; NumericDiffOptions options_; }; diff --git a/extern/ceres/include/ceres/evaluation_callback.h b/extern/ceres/include/ceres/evaluation_callback.h new file mode 100644 index 00000000000..b9f5bbb5194 --- /dev/null +++ b/extern/ceres/include/ceres/evaluation_callback.h @@ -0,0 +1,80 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: mierle@gmail.com (Keir Mierle) + +#ifndef CERES_PUBLIC_EVALUATION_CALLBACK_H_ +#define CERES_PUBLIC_EVALUATION_CALLBACK_H_ + +#include "ceres/internal/port.h" + +namespace ceres { + +// Using this callback interface, Ceres can notify you when it is +// about to evaluate the residuals or jacobians. With the callback, +// you can share computation between residual blocks by doing the +// shared computation in PrepareForEvaluation() before Ceres calls +// CostFunction::Evaluate(). It also enables caching results between a +// pure residual evaluation and a residual & jacobian evaluation, via +// the new_evaluation_point argument. +// +// One use case for this callback is if the cost function compute is +// moved to the GPU. In that case, the prepare call does the actual +// cost function evaluation, and subsequent calls from Ceres to the +// actual cost functions merely copy the results from the GPU onto the +// corresponding blocks for Ceres to plug into the solver. +// +// NOTE: Ceres provides no mechanism to share data other than the +// notification from the callback. Users must provide access to +// pre-computed shared data to their cost functions behind the scenes; +// this all happens without Ceres knowing. +// +// One approach is to put a pointer to the shared data in each cost +// function (recommended) or to use a global shared variable +// (discouraged; bug-prone). As far as Ceres is concerned, it is +// evaluating cost functions like any other; it just so happens that +// behind the scenes the cost functions reuse pre-computed data to +// execute faster. +class CERES_EXPORT EvaluationCallback { + public: + virtual ~EvaluationCallback() {} + + // Called before Ceres requests residuals or jacobians for a given setting of + // the parameters. User parameters (the double* values provided to the cost + // functions) are fixed until the next call to PrepareForEvaluation(). If + // new_evaluation_point == true, then this is a new point that is different + // from the last evaluated point. Otherwise, it is the same point that was + // evaluated previously (either jacobian or residual) and the user can use + // cached results from previous evaluations. + virtual void PrepareForEvaluation(bool evaluate_jacobians, + bool new_evaluation_point) = 0; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_EVALUATION_CALLBACK_H_ diff --git a/extern/ceres/include/ceres/first_order_function.h b/extern/ceres/include/ceres/first_order_function.h new file mode 100644 index 00000000000..1420153b2aa --- /dev/null +++ b/extern/ceres/include/ceres/first_order_function.h @@ -0,0 +1,54 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_FIRST_ORDER_FUNCTION_H_ +#define CERES_PUBLIC_FIRST_ORDER_FUNCTION_H_ + +#include "ceres/internal/port.h" + +namespace ceres { + +// A FirstOrderFunction object implements the evaluation of a function +// and its gradient. +class CERES_EXPORT FirstOrderFunction { + public: + virtual ~FirstOrderFunction() {} + + // cost is never null. gradient may be null. The return value + // indicates whether the evaluation was successful or not. + virtual bool Evaluate(const double* const parameters, + double* cost, + double* gradient) const = 0; + virtual int NumParameters() const = 0; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_FIRST_ORDER_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/gradient_checker.h b/extern/ceres/include/ceres/gradient_checker.h index 6d285daf1d9..b79cf86b314 100644 --- a/extern/ceres/include/ceres/gradient_checker.h +++ b/extern/ceres/include/ceres/gradient_checker.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -34,15 +34,14 @@ #ifndef CERES_PUBLIC_GRADIENT_CHECKER_H_ #define CERES_PUBLIC_GRADIENT_CHECKER_H_ -#include <vector> +#include <memory> #include <string> +#include <vector> #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/local_parameterization.h" #include "glog/logging.h" @@ -64,13 +63,13 @@ namespace ceres { // // 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 { +class CERES_EXPORT GradientChecker { public: // 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 + // local_parameterizations: 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. @@ -80,7 +79,7 @@ class GradientChecker { const NumericDiffOptions& options); // Contains results from a call to Probe for later inspection. - struct ProbeResults { + struct CERES_EXPORT ProbeResults { // The return value of the cost function. bool return_value; @@ -100,10 +99,10 @@ class GradientChecker { // Derivatives as computed by the cost function in local space. std::vector<Matrix> local_jacobians; - // Derivatives as computed by nuerical differentiation in local space. + // Derivatives as computed by numerical differentiation in local space. std::vector<Matrix> numeric_jacobians; - // Derivatives as computed by nuerical differentiation in local space. + // Derivatives as computed by numerical differentiation in local space. std::vector<Matrix> local_numeric_jacobians; // Contains the maximum relative error found in the local Jacobians. @@ -137,11 +136,13 @@ class GradientChecker { ProbeResults* results) const; private: - CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(GradientChecker); + GradientChecker() = delete; + GradientChecker(const GradientChecker&) = delete; + void operator=(const GradientChecker&) = delete; std::vector<const LocalParameterization*> local_parameterizations_; const CostFunction* function_; - internal::scoped_ptr<CostFunction> finite_diff_cost_function_; + std::unique_ptr<CostFunction> finite_diff_cost_function_; }; } // namespace ceres diff --git a/extern/ceres/include/ceres/gradient_problem.h b/extern/ceres/include/ceres/gradient_problem.h index 1226a4cd895..49d605ea2d6 100644 --- a/extern/ceres/include/ceres/gradient_problem.h +++ b/extern/ceres/include/ceres/gradient_problem.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,9 +31,10 @@ #ifndef CERES_PUBLIC_GRADIENT_PROBLEM_H_ #define CERES_PUBLIC_GRADIENT_PROBLEM_H_ -#include "ceres/internal/macros.h" +#include <memory> + +#include "ceres/first_order_function.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/local_parameterization.h" namespace ceres { @@ -105,21 +106,9 @@ class CERES_EXPORT GradientProblem { bool Plus(const double* x, const double* delta, double* x_plus_delta) const; private: - internal::scoped_ptr<FirstOrderFunction> function_; - internal::scoped_ptr<LocalParameterization> parameterization_; - internal::scoped_array<double> scratch_; -}; - -// A FirstOrderFunction object implements the evaluation of a function -// and its gradient. -class CERES_EXPORT FirstOrderFunction { - public: - virtual ~FirstOrderFunction() {} - // cost is never NULL. gradient may be null. - virtual bool Evaluate(const double* const parameters, - double* cost, - double* gradient) const = 0; - virtual int NumParameters() const = 0; + std::unique_ptr<FirstOrderFunction> function_; + std::unique_ptr<LocalParameterization> parameterization_; + std::unique_ptr<double[]> scratch_; }; } // namespace ceres diff --git a/extern/ceres/include/ceres/gradient_problem_solver.h b/extern/ceres/include/ceres/gradient_problem_solver.h index a7d0121ea0c..181699d8fd4 100644 --- a/extern/ceres/include/ceres/gradient_problem_solver.h +++ b/extern/ceres/include/ceres/gradient_problem_solver.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -34,11 +34,11 @@ #include <cmath> #include <string> #include <vector> -#include "ceres/internal/macros.h" + +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/port.h" #include "ceres/iteration_callback.h" #include "ceres/types.h" -#include "ceres/internal/disable_warnings.h" namespace ceres { @@ -54,39 +54,15 @@ class CERES_EXPORT GradientProblemSolver { // // The constants are defined inside types.h struct CERES_EXPORT Options { - // Default constructor that sets up a generic sparse problem. - Options() { - line_search_direction_type = LBFGS; - line_search_type = WOLFE; - nonlinear_conjugate_gradient_type = FLETCHER_REEVES; - max_lbfgs_rank = 20; - use_approximate_eigenvalue_bfgs_scaling = false; - line_search_interpolation_type = CUBIC; - min_line_search_step_size = 1e-9; - line_search_sufficient_function_decrease = 1e-4; - max_line_search_step_contraction = 1e-3; - min_line_search_step_contraction = 0.6; - max_num_line_search_step_size_iterations = 20; - max_num_line_search_direction_restarts = 5; - line_search_sufficient_curvature_decrease = 0.9; - max_line_search_step_expansion = 10.0; - max_num_iterations = 50; - max_solver_time_in_seconds = 1e9; - function_tolerance = 1e-6; - gradient_tolerance = 1e-10; - logging_type = PER_MINIMIZER_ITERATION; - minimizer_progress_to_stdout = false; - } - // Returns true if the options struct has a valid // configuration. Returns false otherwise, and fills in *error // with a message describing the problem. bool IsValid(std::string* error) const; // Minimizer options ---------------------------------------- - LineSearchDirectionType line_search_direction_type; - LineSearchType line_search_type; - NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; + LineSearchDirectionType line_search_direction_type = LBFGS; + LineSearchType line_search_type = WOLFE; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type = FLETCHER_REEVES; // The LBFGS hessian approximation is a low rank approximation to // the inverse of the Hessian matrix. The rank of the @@ -111,8 +87,8 @@ class CERES_EXPORT GradientProblemSolver { // method, please see: // // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with - // Limited Storage". Mathematics of Computation 35 (151): 773–782. - int max_lbfgs_rank; + // Limited Storage". Mathematics of Computation 35 (151): 773-782. + int max_lbfgs_rank = 20; // As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS), // the initial inverse Hessian approximation is taken to be the Identity. @@ -134,18 +110,18 @@ class CERES_EXPORT GradientProblemSolver { // Oren S.S., Self-scaling variable metric (SSVM) algorithms // Part II: Implementation and experiments, Management Science, // 20(5), 863-874, 1974. - bool use_approximate_eigenvalue_bfgs_scaling; + bool use_approximate_eigenvalue_bfgs_scaling = false; // Degree of the polynomial used to approximate the objective // function. Valid values are BISECTION, QUADRATIC and CUBIC. // // BISECTION corresponds to pure backtracking search with no // interpolation. - LineSearchInterpolationType line_search_interpolation_type; + LineSearchInterpolationType line_search_interpolation_type = CUBIC; // If during the line search, the step_size falls below this // value, it is truncated to zero. - double min_line_search_step_size; + double min_line_search_step_size = 1e-9; // Line search parameters. @@ -159,7 +135,7 @@ class CERES_EXPORT GradientProblemSolver { // // f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size // - double line_search_sufficient_function_decrease; + double line_search_sufficient_function_decrease = 1e-4; // In each iteration of the line search, // @@ -169,7 +145,7 @@ class CERES_EXPORT GradientProblemSolver { // // 0 < max_step_contraction < min_step_contraction < 1 // - double max_line_search_step_contraction; + double max_line_search_step_contraction = 1e-3; // In each iteration of the line search, // @@ -179,19 +155,19 @@ class CERES_EXPORT GradientProblemSolver { // // 0 < max_step_contraction < min_step_contraction < 1 // - double min_line_search_step_contraction; + double min_line_search_step_contraction = 0.6; // Maximum number of trial step size iterations during each line search, // if a step size satisfying the search conditions cannot be found within // this number of trials, the line search will terminate. - int max_num_line_search_step_size_iterations; + int max_num_line_search_step_size_iterations = 20; // Maximum number of restarts of the line search direction algorithm before // terminating the optimization. Restarts of the line search direction // algorithm occur when the current algorithm fails to produce a new descent // direction. This typically indicates a numerical failure, or a breakdown // in the validity of the approximations used. - int max_num_line_search_direction_restarts; + int max_num_line_search_direction_restarts = 5; // The strong Wolfe conditions consist of the Armijo sufficient // decrease condition, and an additional requirement that the @@ -204,7 +180,7 @@ class CERES_EXPORT GradientProblemSolver { // // Where f() is the line search objective and f'() is the derivative // of f w.r.t step_size (d f / d step_size). - double line_search_sufficient_curvature_decrease; + double line_search_sufficient_curvature_decrease = 0.9; // During the bracketing phase of the Wolfe search, the step size is // increased until either a point satisfying the Wolfe conditions is @@ -215,36 +191,49 @@ class CERES_EXPORT GradientProblemSolver { // new_step_size <= max_step_expansion * step_size. // // By definition for expansion, max_step_expansion > 1.0. - double max_line_search_step_expansion; + double max_line_search_step_expansion = 10.0; // Maximum number of iterations for the minimizer to run for. - int max_num_iterations; + int max_num_iterations = 50; // Maximum time for which the minimizer should run for. - double max_solver_time_in_seconds; + double max_solver_time_in_seconds = 1e9; // Minimizer terminates when // // (new_cost - old_cost) < function_tolerance * old_cost; // - double function_tolerance; + double function_tolerance = 1e-6; // Minimizer terminates when // // max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance // // This value should typically be 1e-4 * function_tolerance. - double gradient_tolerance; + double gradient_tolerance = 1e-10; + + // Minimizer terminates when + // + // |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance) + // + double parameter_tolerance = 1e-8; // Logging options --------------------------------------------------------- - LoggingType logging_type; + LoggingType logging_type = PER_MINIMIZER_ITERATION; // By default the Minimizer progress is logged to VLOG(1), which // is sent to STDERR depending on the vlog level. If this flag is // set to true, and logging_type is not SILENT, the logging output // is sent to STDOUT. - bool minimizer_progress_to_stdout; + bool minimizer_progress_to_stdout = false; + + // If true, the user's parameter blocks are updated at the end of + // every Minimizer iteration, otherwise they are updated when the + // Minimizer terminates. This is useful if, for example, the user + // wishes to visualize the state of the optimization every + // iteration. + bool update_state_every_iteration = false; // Callbacks that are executed at the end of each iteration of the // Minimizer. An iteration may terminate midway, either due to @@ -265,8 +254,6 @@ class CERES_EXPORT GradientProblemSolver { }; struct CERES_EXPORT Summary { - Summary(); - // A brief one line description of the state of the solver after // termination. std::string BriefReport() const; @@ -278,65 +265,72 @@ class CERES_EXPORT GradientProblemSolver { bool IsSolutionUsable() const; // Minimizer summary ------------------------------------------------- - TerminationType termination_type; + TerminationType termination_type = FAILURE; // Reason why the solver terminated. - std::string message; + std::string message = "ceres::GradientProblemSolve was not called."; // Cost of the problem (value of the objective function) before // the optimization. - double initial_cost; + double initial_cost = -1.0; // Cost of the problem (value of the objective function) after the // optimization. - double final_cost; + double final_cost = -1.0; // IterationSummary for each minimizer iteration in order. std::vector<IterationSummary> iterations; + // Number of times the cost (and not the gradient) was evaluated. + int num_cost_evaluations = -1; + + // Number of times the gradient (and the cost) were evaluated. + int num_gradient_evaluations = -1; + // Sum total of all time spent inside Ceres when Solve is called. - double total_time_in_seconds; + double total_time_in_seconds = -1.0; // Time (in seconds) spent evaluating the cost. - double cost_evaluation_time_in_seconds; + double cost_evaluation_time_in_seconds = -1.0; // Time (in seconds) spent evaluating the gradient. - double gradient_evaluation_time_in_seconds; + double gradient_evaluation_time_in_seconds = -1.0; // Time (in seconds) spent minimizing the interpolating polynomial // to compute the next candidate step size as part of a line search. - double line_search_polynomial_minimization_time_in_seconds; + double line_search_polynomial_minimization_time_in_seconds = -1.0; - // Number of parameters in the probem. - int num_parameters; + // Number of parameters in the problem. + int num_parameters = -1; // Dimension of the tangent space of the problem. - int num_local_parameters; + int num_local_parameters = -1; // Type of line search direction used. - LineSearchDirectionType line_search_direction_type; + LineSearchDirectionType line_search_direction_type = LBFGS; // Type of the line search algorithm used. - LineSearchType line_search_type; + LineSearchType line_search_type = WOLFE; // When performing line search, the degree of the polynomial used // to approximate the objective function. - LineSearchInterpolationType line_search_interpolation_type; + LineSearchInterpolationType line_search_interpolation_type = CUBIC; // If the line search direction is NONLINEAR_CONJUGATE_GRADIENT, // then this indicates the particular variant of non-linear // conjugate gradient used. - NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type = + FLETCHER_REEVES; // If the type of the line search direction is LBFGS, then this // indicates the rank of the Hessian approximation. - int max_lbfgs_rank; + int max_lbfgs_rank = -1; }; // Once a least squares problem has been built, this function takes // the problem and optimizes it based on the values of the options // parameters. Upon return, a detailed summary of the work performed - // by the preprocessor, the non-linear minmizer and the linear + // by the preprocessor, the non-linear minimizer and the linear // solver are reported in the summary object. virtual void Solve(const GradientProblemSolver::Options& options, const GradientProblem& problem, diff --git a/extern/ceres/include/ceres/internal/array_selector.h b/extern/ceres/include/ceres/internal/array_selector.h new file mode 100644 index 00000000000..841797f4c69 --- /dev/null +++ b/extern/ceres/include/ceres/internal/array_selector.h @@ -0,0 +1,95 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2020 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: darius.rueckert@fau.de (Darius Rueckert) +// + +#ifndef CERES_PUBLIC_INTERNAL_ARRAY_SELECTOR_H_ +#define CERES_PUBLIC_INTERNAL_ARRAY_SELECTOR_H_ + +#include <array> +#include <vector> + +#include "ceres/internal/fixed_array.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// StaticFixedArray selects the best array implementation based on template +// arguments. If the size is not known at compile-time, pass +// ceres::DYNAMIC as a size-template argument. +// +// Three different containers are selected in different scenarios: +// +// num_elements == DYNAMIC: +// -> ceres::internal::FixedArray<T, max_stack_size>(size) + +// num_elements != DYNAMIC && num_elements <= max_stack_size +// -> std::array<T,num_elements> + +// num_elements != DYNAMIC && num_elements > max_stack_size +// -> std::vector<T>(num_elements) +// +template <typename T, + int num_elements, + int max_num_elements_on_stack, + bool dynamic = (num_elements == DYNAMIC), + bool fits_on_stack = (num_elements <= max_num_elements_on_stack)> +struct ArraySelector {}; + +template <typename T, + int num_elements, + int max_num_elements_on_stack, + bool fits_on_stack> +struct ArraySelector<T, + num_elements, + max_num_elements_on_stack, + true, + fits_on_stack> + : ceres::internal::FixedArray<T, max_num_elements_on_stack> { + ArraySelector(int s) + : ceres::internal::FixedArray<T, max_num_elements_on_stack>(s) {} +}; + +template <typename T, int num_elements, int max_num_elements_on_stack> +struct ArraySelector<T, num_elements, max_num_elements_on_stack, false, true> + : std::array<T, num_elements> { + ArraySelector(int s) { CHECK_EQ(s, num_elements); } +}; + +template <typename T, int num_elements, int max_num_elements_on_stack> +struct ArraySelector<T, num_elements, max_num_elements_on_stack, false, false> + : std::vector<T> { + ArraySelector(int s) : std::vector<T>(s) { CHECK_EQ(s, num_elements); } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_ARRAY_SELECTOR_H_ diff --git a/extern/ceres/include/ceres/internal/autodiff.h b/extern/ceres/include/ceres/internal/autodiff.h index 136152a36cd..cb7b1aca5b9 100644 --- a/extern/ceres/include/ceres/internal/autodiff.h +++ b/extern/ceres/include/ceres/internal/autodiff.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -30,10 +30,10 @@ // // Computation of the Jacobian matrix for vector-valued functions of multiple // variables, using automatic differentiation based on the implementation of -// dual numbers in jet.h. Before reading the rest of this file, it is adivsable +// dual numbers in jet.h. Before reading the rest of this file, it is advisable // to read jet.h's header comment in detail. // -// The helper wrapper AutoDiff::Differentiate() computes the jacobian of +// The helper wrapper AutoDifferentiate() computes the jacobian of // functors with templated operator() taking this form: // // struct F { @@ -57,7 +57,7 @@ // [ * ] // // Similar to the 2-parameter example for f described in jet.h, computing the -// jacobian dy/dx is done by substutiting a suitable jet object for x and all +// jacobian dy/dx is done by substituting a suitable jet object for x and all // intermediate steps of the computation of F. Since x is has 4 dimensions, use // a Jet<double, 4>. // @@ -142,16 +142,33 @@ #include <stddef.h> -#include "ceres/jet.h" +#include <array> +#include <utility> + +#include "ceres/internal/array_selector.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" +#include "ceres/internal/parameter_dims.h" #include "ceres/internal/variadic_evaluate.h" +#include "ceres/jet.h" +#include "ceres/types.h" #include "glog/logging.h" +// If the number of parameters exceeds this values, the corresponding jets are +// placed on the heap. This will reduce performance by a factor of 2-5 on +// current compilers. +#ifndef CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK +#define CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK 50 +#endif + +#ifndef CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK +#define CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK 20 +#endif + namespace ceres { namespace internal { -// Extends src by a 1st order pertubation for every dimension and puts it in +// Extends src by a 1st order perturbation for every dimension and puts it in // dst. The size of src is N. Since this is also used for perturbations in // blocked arrays, offset is used to shift which part of the jet the // perturbation occurs. This is used to set up the extended x augmented by an @@ -165,21 +182,62 @@ namespace internal { // // is what would get put in dst if N was 3, offset was 3, and the jet type JetT // was 8-dimensional. -template <typename JetT, typename T, int N> -inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) { - DCHECK(src); - DCHECK(dst); - for (int j = 0; j < N; ++j) { - dst[j].a = src[j]; - dst[j].v.setZero(); - dst[j].v[offset + j] = T(1.0); +template <int j, int N, int Offset, typename T, typename JetT> +struct Make1stOrderPerturbation { + public: + inline static void Apply(const T* src, JetT* dst) { + if (j == 0) { + DCHECK(src); + DCHECK(dst); + } + dst[j] = JetT(src[j], j + Offset); + Make1stOrderPerturbation<j + 1, N, Offset, T, JetT>::Apply(src, dst); } -} +}; + +template <int N, int Offset, typename T, typename JetT> +struct Make1stOrderPerturbation<N, N, Offset, T, JetT> { + public: + static void Apply(const T* /*src*/, JetT* /*dst*/) {} +}; + +// Calls Make1stOrderPerturbation for every parameter block. +// +// Example: +// If one having three parameter blocks with dimensions (3, 2, 4), the call +// Make1stOrderPerturbations<integer_sequence<3, 2, 4>::Apply(params, x); +// will result in the following calls to Make1stOrderPerturbation: +// Make1stOrderPerturbation<0, 3, 0>::Apply(params[0], x + 0); +// Make1stOrderPerturbation<0, 2, 3>::Apply(params[1], x + 3); +// Make1stOrderPerturbation<0, 4, 5>::Apply(params[2], x + 5); +template <typename Seq, int ParameterIdx = 0, int Offset = 0> +struct Make1stOrderPerturbations; + +template <int N, int... Ns, int ParameterIdx, int Offset> +struct Make1stOrderPerturbations<std::integer_sequence<int, N, Ns...>, + ParameterIdx, + Offset> { + template <typename T, typename JetT> + inline static void Apply(T const* const* parameters, JetT* x) { + Make1stOrderPerturbation<0, N, Offset, T, JetT>::Apply( + parameters[ParameterIdx], x + Offset); + Make1stOrderPerturbations<std::integer_sequence<int, Ns...>, + ParameterIdx + 1, + Offset + N>::Apply(parameters, x); + } +}; + +// End of 'recursion'. Nothing more to do. +template <int ParameterIdx, int Total> +struct Make1stOrderPerturbations<std::integer_sequence<int>, ParameterIdx, Total> { + template <typename T, typename JetT> + static void Apply(T const* const* /* NOT USED */, JetT* /* NOT USED */) {} +}; // Takes the 0th order part of src, assumed to be a Jet type, and puts it in // dst. This is used to pick out the "vector" part of the extended y. template <typename JetT, typename T> -inline void Take0thOrderPart(int M, const JetT *src, T dst) { +inline void Take0thOrderPart(int M, const JetT* src, T dst) { DCHECK(src); for (int i = 0; i < M; ++i) { dst[i] = src[i].a; @@ -188,128 +246,117 @@ inline void Take0thOrderPart(int M, const JetT *src, T dst) { // Takes N 1st order parts, starting at index N0, and puts them in the M x N // matrix 'dst'. This is used to pick out the "matrix" parts of the extended y. -template <typename JetT, typename T, int N0, int N> -inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { +template <int N0, int N, typename JetT, typename T> +inline void Take1stOrderPart(const int M, const JetT* src, T* dst) { DCHECK(src); DCHECK(dst); for (int i = 0; i < M; ++i) { - Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) = + Eigen::Map<Eigen::Matrix<T, N, 1>>(dst + N * i, N) = src[i].v.template segment<N>(N0); } } -// This is in a struct because default template parameters on a -// function are not supported in C++03 (though it is available in -// C++0x). N0 through N5 are the dimension of the input arguments to -// the user supplied functor. -template <typename Functor, typename T, - int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, - int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> -struct AutoDiff { - static bool Differentiate(const Functor& functor, - T const *const *parameters, - int num_outputs, - T *function_value, - T **jacobians) { - // This block breaks the 80 column rule to keep it somewhat readable. - DCHECK_GT(num_outputs, 0); - DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || - ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || - ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT - << "Zero block cannot precede a non-zero block. Block sizes are " - << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " - << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " - << N8 << ", " << N9; +// Calls Take1stOrderPart for every parameter block. +// +// Example: +// If one having three parameter blocks with dimensions (3, 2, 4), the call +// Take1stOrderParts<integer_sequence<3, 2, 4>::Apply(num_outputs, +// output, +// jacobians); +// will result in the following calls to Take1stOrderPart: +// if (jacobians[0]) { +// Take1stOrderPart<0, 3>(num_outputs, output, jacobians[0]); +// } +// if (jacobians[1]) { +// Take1stOrderPart<3, 2>(num_outputs, output, jacobians[1]); +// } +// if (jacobians[2]) { +// Take1stOrderPart<5, 4>(num_outputs, output, jacobians[2]); +// } +template <typename Seq, int ParameterIdx = 0, int Offset = 0> +struct Take1stOrderParts; - typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT; - FixedArray<JetT, (256 * 7) / sizeof(JetT)> x( - N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs); +template <int N, int... Ns, int ParameterIdx, int Offset> +struct Take1stOrderParts<std::integer_sequence<int, N, Ns...>, + ParameterIdx, + Offset> { + template <typename JetT, typename T> + inline static void Apply(int num_outputs, JetT* output, T** jacobians) { + if (jacobians[ParameterIdx]) { + Take1stOrderPart<Offset, N>(num_outputs, output, jacobians[ParameterIdx]); + } + Take1stOrderParts<std::integer_sequence<int, Ns...>, + ParameterIdx + 1, + Offset + N>::Apply(num_outputs, output, jacobians); + } +}; - // These are the positions of the respective jets in the fixed array x. - const int jet0 = 0; - const int jet1 = N0; - const int jet2 = N0 + N1; - const int jet3 = N0 + N1 + N2; - const int jet4 = N0 + N1 + N2 + N3; - const int jet5 = N0 + N1 + N2 + N3 + N4; - const int jet6 = N0 + N1 + N2 + N3 + N4 + N5; - const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6; - const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7; - const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8; +// End of 'recursion'. Nothing more to do. +template <int ParameterIdx, int Offset> +struct Take1stOrderParts<std::integer_sequence<int>, ParameterIdx, Offset> { + template <typename T, typename JetT> + static void Apply(int /* NOT USED*/, + JetT* /* NOT USED*/, + T** /* NOT USED */) {} +}; - const JetT *unpacked_parameters[10] = { - x.get() + jet0, - x.get() + jet1, - x.get() + jet2, - x.get() + jet3, - x.get() + jet4, - x.get() + jet5, - x.get() + jet6, - x.get() + jet7, - x.get() + jet8, - x.get() + jet9, - }; +template <int kNumResiduals, + typename ParameterDims, + typename Functor, + typename T> +inline bool AutoDifferentiate(const Functor& functor, + T const* const* parameters, + int dynamic_num_outputs, + T* function_value, + T** jacobians) { + typedef Jet<T, ParameterDims::kNumParameters> JetT; + using Parameters = typename ParameterDims::Parameters; - JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; + if (kNumResiduals != DYNAMIC) { + DCHECK_EQ(kNumResiduals, dynamic_num_outputs); + } -#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ - if (N ## i) { \ - internal::Make1stOrderPerturbation<JetT, T, N ## i>( \ - jet ## i, \ - parameters[i], \ - x.get() + jet ## i); \ - } - CERES_MAKE_1ST_ORDER_PERTURBATION(0); - CERES_MAKE_1ST_ORDER_PERTURBATION(1); - CERES_MAKE_1ST_ORDER_PERTURBATION(2); - CERES_MAKE_1ST_ORDER_PERTURBATION(3); - CERES_MAKE_1ST_ORDER_PERTURBATION(4); - CERES_MAKE_1ST_ORDER_PERTURBATION(5); - CERES_MAKE_1ST_ORDER_PERTURBATION(6); - CERES_MAKE_1ST_ORDER_PERTURBATION(7); - CERES_MAKE_1ST_ORDER_PERTURBATION(8); - CERES_MAKE_1ST_ORDER_PERTURBATION(9); -#undef CERES_MAKE_1ST_ORDER_PERTURBATION + ArraySelector<JetT, + ParameterDims::kNumParameters, + CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK> + parameters_as_jets(ParameterDims::kNumParameters); - if (!VariadicEvaluate<Functor, JetT, - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call( - functor, unpacked_parameters, output)) { - return false; - } + // Pointers to the beginning of each parameter block + std::array<JetT*, ParameterDims::kNumParameterBlocks> unpacked_parameters = + ParameterDims::GetUnpackedParameters(parameters_as_jets.data()); - internal::Take0thOrderPart(num_outputs, output, function_value); + // If the number of residuals is fixed, we use the template argument as the + // number of outputs. Otherwise we use the num_outputs parameter. Note: The + // ?-operator here is compile-time evaluated, therefore num_outputs is also + // a compile-time constant for functors with fixed residuals. + const int num_outputs = + kNumResiduals == DYNAMIC ? dynamic_num_outputs : kNumResiduals; + DCHECK_GT(num_outputs, 0); -#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \ - if (N ## i) { \ - if (jacobians[i]) { \ - internal::Take1stOrderPart<JetT, T, \ - jet ## i, \ - N ## i>(num_outputs, \ - output, \ - jacobians[i]); \ - } \ - } - CERES_TAKE_1ST_ORDER_PERTURBATION(0); - CERES_TAKE_1ST_ORDER_PERTURBATION(1); - CERES_TAKE_1ST_ORDER_PERTURBATION(2); - CERES_TAKE_1ST_ORDER_PERTURBATION(3); - CERES_TAKE_1ST_ORDER_PERTURBATION(4); - CERES_TAKE_1ST_ORDER_PERTURBATION(5); - CERES_TAKE_1ST_ORDER_PERTURBATION(6); - CERES_TAKE_1ST_ORDER_PERTURBATION(7); - CERES_TAKE_1ST_ORDER_PERTURBATION(8); - CERES_TAKE_1ST_ORDER_PERTURBATION(9); -#undef CERES_TAKE_1ST_ORDER_PERTURBATION - return true; + ArraySelector<JetT, kNumResiduals, CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK> + residuals_as_jets(num_outputs); + + // Invalidate the output Jets, so that we can detect if the user + // did not assign values to all of them. + for (int i = 0; i < num_outputs; ++i) { + residuals_as_jets[i].a = kImpossibleValue; + residuals_as_jets[i].v.setConstant(kImpossibleValue); } -}; + + Make1stOrderPerturbations<Parameters>::Apply(parameters, + parameters_as_jets.data()); + + if (!VariadicEvaluate<ParameterDims>( + functor, unpacked_parameters.data(), residuals_as_jets.data())) { + return false; + } + + Take0thOrderPart(num_outputs, residuals_as_jets.data(), function_value); + Take1stOrderParts<Parameters>::Apply( + num_outputs, residuals_as_jets.data(), jacobians); + + return true; +} } // namespace internal } // namespace ceres diff --git a/extern/ceres/include/ceres/internal/disable_warnings.h b/extern/ceres/include/ceres/internal/disable_warnings.h index 094124f7159..fd848feec0f 100644 --- a/extern/ceres/include/ceres/internal/disable_warnings.h +++ b/extern/ceres/include/ceres/internal/disable_warnings.h @@ -35,7 +35,7 @@ #ifdef _MSC_VER #pragma warning( push ) -// Disable the warning C4251 which is trigerred by stl classes in +// Disable the warning C4251 which is triggered by stl classes in // Ceres' public interface. To quote MSDN: "C4251 can be ignored " // "if you are deriving from a type in the Standard C++ Library" #pragma warning( disable : 4251 ) diff --git a/extern/ceres/include/ceres/internal/eigen.h b/extern/ceres/include/ceres/internal/eigen.h index 7138804ace4..59545dfd9c9 100644 --- a/extern/ceres/include/ceres/internal/eigen.h +++ b/extern/ceres/include/ceres/internal/eigen.h @@ -52,40 +52,27 @@ typedef Eigen::Matrix<double, Eigen::ColMajor> ColMajorMatrix; typedef Eigen::Map<ColMajorMatrix, 0, - Eigen::Stride<Eigen::Dynamic, 1> > ColMajorMatrixRef; + Eigen::Stride<Eigen::Dynamic, 1>> ColMajorMatrixRef; typedef Eigen::Map<const ColMajorMatrix, 0, - Eigen::Stride<Eigen::Dynamic, 1> > ConstColMajorMatrixRef; - - + Eigen::Stride<Eigen::Dynamic, 1>> ConstColMajorMatrixRef; // C++ does not support templated typdefs, thus the need for this // struct so that we can support statically sized Matrix and Maps. -template <int num_rows = Eigen::Dynamic, int num_cols = Eigen::Dynamic> + template <int num_rows = Eigen::Dynamic, int num_cols = Eigen::Dynamic> struct EigenTypes { - typedef Eigen::Matrix <double, num_rows, num_cols, Eigen::RowMajor> - Matrix; - - typedef Eigen::Map< - Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> > - MatrixRef; - - typedef Eigen::Matrix <double, num_rows, 1> - Vector; - - typedef Eigen::Map < - Eigen::Matrix<double, num_rows, 1> > - VectorRef; - - - typedef Eigen::Map< - const Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> > - ConstMatrixRef; - - typedef Eigen::Map < - const Eigen::Matrix<double, num_rows, 1> > - ConstVectorRef; + typedef Eigen::Matrix<double, + num_rows, + num_cols, + num_cols == 1 ? Eigen::ColMajor : Eigen::RowMajor> + Matrix; + + typedef Eigen::Map<Matrix> MatrixRef; + typedef Eigen::Map<const Matrix> ConstMatrixRef; + typedef Eigen::Matrix<double, num_rows, 1> Vector; + typedef Eigen::Map<Eigen::Matrix<double, num_rows, 1>> VectorRef; + typedef Eigen::Map<const Eigen::Matrix<double, num_rows, 1>> ConstVectorRef; }; } // namespace ceres diff --git a/extern/ceres/include/ceres/internal/fixed_array.h b/extern/ceres/include/ceres/internal/fixed_array.h index 387298c58d0..f8ef02d40e8 100644 --- a/extern/ceres/include/ceres/internal/fixed_array.h +++ b/extern/ceres/include/ceres/internal/fixed_array.h @@ -1,189 +1,466 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://ceres-solver.org/ +// Copyright 2018 The Abseil Authors. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// * Neither the name of Google Inc. nor the names of its contributors may be -// used to endorse or promote products derived from this software without -// specific prior written permission. +// https://www.apache.org/licenses/LICENSE-2.0 // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. // -// Author: rennie@google.com (Jeffrey Rennie) -// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray +// ----------------------------------------------------------------------------- +// File: fixed_array.h +// ----------------------------------------------------------------------------- +// +// A `FixedArray<T>` represents a non-resizable array of `T` where the length of +// the array can be determined at run-time. It is a good replacement for +// non-standard and deprecated uses of `alloca()` and variable length arrays +// within the GCC extension. (See +// https://gcc.gnu.org/onlinedocs/gcc/Variable-Length.html). +// +// `FixedArray` allocates small arrays inline, keeping performance fast by +// avoiding heap operations. It also helps reduce the chances of +// accidentally overflowing your stack if large input is passed to +// your function. #ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ +#include <algorithm> +#include <array> #include <cstddef> -#include "Eigen/Core" -#include "ceres/internal/macros.h" -#include "ceres/internal/manual_constructor.h" +#include <memory> +#include <tuple> +#include <type_traits> + +#include <Eigen/Core> // For Eigen::aligned_allocator + +#include "ceres/internal/memory.h" #include "glog/logging.h" namespace ceres { namespace internal { -// A FixedArray<T> represents a non-resizable array of T where the -// length of the array does not need to be a compile time constant. -// -// FixedArray allocates small arrays inline, and large arrays on -// the heap. It is a good replacement for non-standard and deprecated -// uses of alloca() and variable length arrays (a GCC extension). -// -// FixedArray keeps performance fast for small arrays, because it -// avoids heap operations. It also helps reduce the chances of -// accidentally overflowing your stack if large input is passed to -// your function. +constexpr static auto kFixedArrayUseDefault = static_cast<size_t>(-1); + +// The default fixed array allocator. // -// Also, FixedArray is useful for writing portable code. Not all -// compilers support arrays of dynamic size. +// As one can not easily detect if a struct contains or inherits from a fixed +// size Eigen type, to be safe the Eigen::aligned_allocator is used by default. +// But trivial types can never contain Eigen types, so std::allocator is used to +// safe some heap memory. +template <typename T> +using FixedArrayDefaultAllocator = + typename std::conditional<std::is_trivial<T>::value, + std::allocator<T>, + Eigen::aligned_allocator<T>>::type; -// Most users should not specify an inline_elements argument and let -// FixedArray<> automatically determine the number of elements -// to store inline based on sizeof(T). +// ----------------------------------------------------------------------------- +// FixedArray +// ----------------------------------------------------------------------------- +// +// A `FixedArray` provides a run-time fixed-size array, allocating a small array +// inline for efficiency. // -// If inline_elements is specified, the FixedArray<> implementation -// will store arrays of length <= inline_elements inline. +// Most users should not specify an `inline_elements` argument and let +// `FixedArray` automatically determine the number of elements +// to store inline based on `sizeof(T)`. If `inline_elements` is specified, the +// `FixedArray` implementation will use inline storage for arrays with a +// length <= `inline_elements`. // -// Finally note that unlike vector<T> FixedArray<T> will not zero-initialize -// simple types like int, double, bool, etc. +// Note that a `FixedArray` constructed with a `size_type` argument will +// default-initialize its values by leaving trivially constructible types +// uninitialized (e.g. int, int[4], double), and others default-constructed. +// This matches the behavior of c-style arrays and `std::array`, but not +// `std::vector`. // -// Non-POD types will be default-initialized just like regular vectors or -// arrays. +// Note that `FixedArray` does not provide a public allocator; if it requires a +// heap allocation, it will do so with global `::operator new[]()` and +// `::operator delete[]()`, even if T provides class-scope overrides for these +// operators. +template <typename T, + size_t N = kFixedArrayUseDefault, + typename A = FixedArrayDefaultAllocator<T>> +class FixedArray { + static_assert(!std::is_array<T>::value || std::extent<T>::value > 0, + "Arrays with unknown bounds cannot be used with FixedArray."); -#if defined(_WIN64) - typedef __int64 ssize_t; -#elif defined(_WIN32) - typedef __int32 ssize_t; -#endif + static constexpr size_t kInlineBytesDefault = 256; + + using AllocatorTraits = std::allocator_traits<A>; + // std::iterator_traits isn't guaranteed to be SFINAE-friendly until C++17, + // but this seems to be mostly pedantic. + template <typename Iterator> + using EnableIfForwardIterator = typename std::enable_if<std::is_convertible< + typename std::iterator_traits<Iterator>::iterator_category, + std::forward_iterator_tag>::value>::type; + static constexpr bool DefaultConstructorIsNonTrivial() { + return !std::is_trivially_default_constructible<StorageElement>::value; + } -template <typename T, ssize_t inline_elements = -1> -class FixedArray { public: - // For playing nicely with stl: - typedef T value_type; - typedef T* iterator; - typedef T const* const_iterator; - typedef T& reference; - typedef T const& const_reference; - typedef T* pointer; - typedef std::ptrdiff_t difference_type; - typedef size_t size_type; - - // REQUIRES: n >= 0 - // Creates an array object that can store "n" elements. - // - // FixedArray<T> will not zero-initialiaze POD (simple) types like int, - // double, bool, etc. - // Non-POD types will be default-initialized just like regular vectors or - // arrays. - explicit FixedArray(size_type n); - - // Releases any resources. - ~FixedArray(); - - // Returns the length of the array. - inline size_type size() const { return size_; } - - // Returns the memory size of the array in bytes. - inline size_t memsize() const { return size_ * sizeof(T); } - - // Returns a pointer to the underlying element array. - inline const T* get() const { return &array_[0].element; } - inline T* get() { return &array_[0].element; } + using allocator_type = typename AllocatorTraits::allocator_type; + using value_type = typename AllocatorTraits::value_type; + using pointer = typename AllocatorTraits::pointer; + using const_pointer = typename AllocatorTraits::const_pointer; + using reference = value_type&; + using const_reference = const value_type&; + using size_type = typename AllocatorTraits::size_type; + using difference_type = typename AllocatorTraits::difference_type; + using iterator = pointer; + using const_iterator = const_pointer; + using reverse_iterator = std::reverse_iterator<iterator>; + using const_reverse_iterator = std::reverse_iterator<const_iterator>; + + static constexpr size_type inline_elements = + (N == kFixedArrayUseDefault ? kInlineBytesDefault / sizeof(value_type) + : static_cast<size_type>(N)); + + FixedArray(const FixedArray& other, + const allocator_type& a = allocator_type()) + : FixedArray(other.begin(), other.end(), a) {} + + FixedArray(FixedArray&& other, const allocator_type& a = allocator_type()) + : FixedArray(std::make_move_iterator(other.begin()), + std::make_move_iterator(other.end()), + a) {} + + // Creates an array object that can store `n` elements. + // Note that trivially constructible elements will be uninitialized. + explicit FixedArray(size_type n, const allocator_type& a = allocator_type()) + : storage_(n, a) { + if (DefaultConstructorIsNonTrivial()) { + ConstructRange(storage_.alloc(), storage_.begin(), storage_.end()); + } + } + // Creates an array initialized with `n` copies of `val`. + FixedArray(size_type n, + const value_type& val, + const allocator_type& a = allocator_type()) + : storage_(n, a) { + ConstructRange(storage_.alloc(), storage_.begin(), storage_.end(), val); + } + + // Creates an array initialized with the size and contents of `init_list`. + FixedArray(std::initializer_list<value_type> init_list, + const allocator_type& a = allocator_type()) + : FixedArray(init_list.begin(), init_list.end(), a) {} + + // Creates an array initialized with the elements from the input + // range. The array's size will always be `std::distance(first, last)`. + // REQUIRES: Iterator must be a forward_iterator or better. + template <typename Iterator, EnableIfForwardIterator<Iterator>* = nullptr> + FixedArray(Iterator first, + Iterator last, + const allocator_type& a = allocator_type()) + : storage_(std::distance(first, last), a) { + CopyRange(storage_.alloc(), storage_.begin(), first, last); + } + + ~FixedArray() noexcept { + for (auto* cur = storage_.begin(); cur != storage_.end(); ++cur) { + AllocatorTraits::destroy(storage_.alloc(), cur); + } + } + + // Assignments are deleted because they break the invariant that the size of a + // `FixedArray` never changes. + void operator=(FixedArray&&) = delete; + void operator=(const FixedArray&) = delete; + + // FixedArray::size() + // + // Returns the length of the fixed array. + size_type size() const { return storage_.size(); } + + // FixedArray::max_size() + // + // Returns the largest possible value of `std::distance(begin(), end())` for a + // `FixedArray<T>`. This is equivalent to the most possible addressable bytes + // over the number of bytes taken by T. + constexpr size_type max_size() const { + return (std::numeric_limits<difference_type>::max)() / sizeof(value_type); + } + + // FixedArray::empty() + // + // Returns whether or not the fixed array is empty. + bool empty() const { return size() == 0; } + + // FixedArray::memsize() + // + // Returns the memory size of the fixed array in bytes. + size_t memsize() const { return size() * sizeof(value_type); } + + // FixedArray::data() + // + // Returns a const T* pointer to elements of the `FixedArray`. This pointer + // can be used to access (but not modify) the contained elements. + const_pointer data() const { return AsValueType(storage_.begin()); } + + // Overload of FixedArray::data() to return a T* pointer to elements of the + // fixed array. This pointer can be used to access and modify the contained + // elements. + pointer data() { return AsValueType(storage_.begin()); } + + // FixedArray::operator[] + // + // Returns a reference the ith element of the fixed array. // REQUIRES: 0 <= i < size() - // Returns a reference to the "i"th element. - inline T& operator[](size_type i) { - DCHECK_LT(i, size_); - return array_[i].element; + reference operator[](size_type i) { + DCHECK_LT(i, size()); + return data()[i]; } + // Overload of FixedArray::operator()[] to return a const reference to the + // ith element of the fixed array. // REQUIRES: 0 <= i < size() - // Returns a reference to the "i"th element. - inline const T& operator[](size_type i) const { - DCHECK_LT(i, size_); - return array_[i].element; + const_reference operator[](size_type i) const { + DCHECK_LT(i, size()); + return data()[i]; + } + + // FixedArray::front() + // + // Returns a reference to the first element of the fixed array. + reference front() { return *begin(); } + + // Overload of FixedArray::front() to return a reference to the first element + // of a fixed array of const values. + const_reference front() const { return *begin(); } + + // FixedArray::back() + // + // Returns a reference to the last element of the fixed array. + reference back() { return *(end() - 1); } + + // Overload of FixedArray::back() to return a reference to the last element + // of a fixed array of const values. + const_reference back() const { return *(end() - 1); } + + // FixedArray::begin() + // + // Returns an iterator to the beginning of the fixed array. + iterator begin() { return data(); } + + // Overload of FixedArray::begin() to return a const iterator to the + // beginning of the fixed array. + const_iterator begin() const { return data(); } + + // FixedArray::cbegin() + // + // Returns a const iterator to the beginning of the fixed array. + const_iterator cbegin() const { return begin(); } + + // FixedArray::end() + // + // Returns an iterator to the end of the fixed array. + iterator end() { return data() + size(); } + + // Overload of FixedArray::end() to return a const iterator to the end of the + // fixed array. + const_iterator end() const { return data() + size(); } + + // FixedArray::cend() + // + // Returns a const iterator to the end of the fixed array. + const_iterator cend() const { return end(); } + + // FixedArray::rbegin() + // + // Returns a reverse iterator from the end of the fixed array. + reverse_iterator rbegin() { return reverse_iterator(end()); } + + // Overload of FixedArray::rbegin() to return a const reverse iterator from + // the end of the fixed array. + const_reverse_iterator rbegin() const { + return const_reverse_iterator(end()); } - inline iterator begin() { return &array_[0].element; } - inline iterator end() { return &array_[size_].element; } + // FixedArray::crbegin() + // + // Returns a const reverse iterator from the end of the fixed array. + const_reverse_iterator crbegin() const { return rbegin(); } - inline const_iterator begin() const { return &array_[0].element; } - inline const_iterator end() const { return &array_[size_].element; } + // FixedArray::rend() + // + // Returns a reverse iterator from the beginning of the fixed array. + reverse_iterator rend() { return reverse_iterator(begin()); } + + // Overload of FixedArray::rend() for returning a const reverse iterator + // from the beginning of the fixed array. + const_reverse_iterator rend() const { + return const_reverse_iterator(begin()); + } + + // FixedArray::crend() + // + // Returns a reverse iterator from the beginning of the fixed array. + const_reverse_iterator crend() const { return rend(); } + + // FixedArray::fill() + // + // Assigns the given `value` to all elements in the fixed array. + void fill(const value_type& val) { std::fill(begin(), end(), val); } + + // Relational operators. Equality operators are elementwise using + // `operator==`, while order operators order FixedArrays lexicographically. + friend bool operator==(const FixedArray& lhs, const FixedArray& rhs) { + return std::equal(lhs.begin(), lhs.end(), rhs.begin(), rhs.end()); + } + + friend bool operator!=(const FixedArray& lhs, const FixedArray& rhs) { + return !(lhs == rhs); + } + + friend bool operator<(const FixedArray& lhs, const FixedArray& rhs) { + return std::lexicographical_compare( + lhs.begin(), lhs.end(), rhs.begin(), rhs.end()); + } + + friend bool operator>(const FixedArray& lhs, const FixedArray& rhs) { + return rhs < lhs; + } + + friend bool operator<=(const FixedArray& lhs, const FixedArray& rhs) { + return !(rhs < lhs); + } + + friend bool operator>=(const FixedArray& lhs, const FixedArray& rhs) { + return !(lhs < rhs); + } private: - // Container to hold elements of type T. This is necessary to handle - // the case where T is a a (C-style) array. The size of InnerContainer - // and T must be the same, otherwise callers' assumptions about use - // of this code will be broken. - struct InnerContainer { - T element; + // StorageElement + // + // For FixedArrays with a C-style-array value_type, StorageElement is a POD + // wrapper struct called StorageElementWrapper that holds the value_type + // instance inside. This is needed for construction and destruction of the + // entire array regardless of how many dimensions it has. For all other cases, + // StorageElement is just an alias of value_type. + // + // Maintainer's Note: The simpler solution would be to simply wrap value_type + // in a struct whether it's an array or not. That causes some paranoid + // diagnostics to misfire, believing that 'data()' returns a pointer to a + // single element, rather than the packed array that it really is. + // e.g.: + // + // FixedArray<char> buf(1); + // sprintf(buf.data(), "foo"); + // + // error: call to int __builtin___sprintf_chk(etc...) + // will always overflow destination buffer [-Werror] + // + template <typename OuterT, + typename InnerT = typename std::remove_extent<OuterT>::type, + size_t InnerN = std::extent<OuterT>::value> + struct StorageElementWrapper { + InnerT array[InnerN]; }; - // How many elements should we store inline? - // a. If not specified, use a default of 256 bytes (256 bytes - // seems small enough to not cause stack overflow or unnecessary - // stack pollution, while still allowing stack allocation for - // reasonably long character arrays. - // b. Never use 0 length arrays (not ISO C++) - static const size_type S1 = ((inline_elements < 0) - ? (256/sizeof(T)) : inline_elements); - static const size_type S2 = (S1 <= 0) ? 1 : S1; - static const size_type kInlineElements = S2; - - size_type const size_; - InnerContainer* const array_; - - // Allocate some space, not an array of elements of type T, so that we can - // skip calling the T constructors and destructors for space we never use. - ManualConstructor<InnerContainer> inline_space_[kInlineElements]; -}; + using StorageElement = + typename std::conditional<std::is_array<value_type>::value, + StorageElementWrapper<value_type>, + value_type>::type; -// Implementation details follow - -template <class T, ssize_t S> -inline FixedArray<T, S>::FixedArray(typename FixedArray<T, S>::size_type n) - : size_(n), - array_((n <= kInlineElements - ? reinterpret_cast<InnerContainer*>(inline_space_) - : new InnerContainer[n])) { - // Construct only the elements actually used. - if (array_ == reinterpret_cast<InnerContainer*>(inline_space_)) { - for (size_t i = 0; i != size_; ++i) { - inline_space_[i].Init(); - } + static pointer AsValueType(pointer ptr) { return ptr; } + static pointer AsValueType(StorageElementWrapper<value_type>* ptr) { + return std::addressof(ptr->array); } -} -template <class T, ssize_t S> -inline FixedArray<T, S>::~FixedArray() { - if (array_ != reinterpret_cast<InnerContainer*>(inline_space_)) { - delete[] array_; - } else { - for (size_t i = 0; i != size_; ++i) { - inline_space_[i].Destroy(); + static_assert(sizeof(StorageElement) == sizeof(value_type), ""); + static_assert(alignof(StorageElement) == alignof(value_type), ""); + + class NonEmptyInlinedStorage { + public: + StorageElement* data() { return reinterpret_cast<StorageElement*>(buff_); } + void AnnotateConstruct(size_type) {} + void AnnotateDestruct(size_type) {} + + // #ifdef ADDRESS_SANITIZER + // void* RedzoneBegin() { return &redzone_begin_; } + // void* RedzoneEnd() { return &redzone_end_ + 1; } + // #endif // ADDRESS_SANITIZER + + private: + // ADDRESS_SANITIZER_REDZONE(redzone_begin_); + alignas(StorageElement) char buff_[sizeof(StorageElement[inline_elements])]; + // ADDRESS_SANITIZER_REDZONE(redzone_end_); + }; + + class EmptyInlinedStorage { + public: + StorageElement* data() { return nullptr; } + void AnnotateConstruct(size_type) {} + void AnnotateDestruct(size_type) {} + }; + + using InlinedStorage = + typename std::conditional<inline_elements == 0, + EmptyInlinedStorage, + NonEmptyInlinedStorage>::type; + + // Storage + // + // An instance of Storage manages the inline and out-of-line memory for + // instances of FixedArray. This guarantees that even when construction of + // individual elements fails in the FixedArray constructor body, the + // destructor for Storage will still be called and out-of-line memory will be + // properly deallocated. + // + class Storage : public InlinedStorage { + public: + Storage(size_type n, const allocator_type& a) + : size_alloc_(n, a), data_(InitializeData()) {} + + ~Storage() noexcept { + if (UsingInlinedStorage(size())) { + InlinedStorage::AnnotateDestruct(size()); + } else { + AllocatorTraits::deallocate(alloc(), AsValueType(begin()), size()); + } } - } -} + + size_type size() const { return std::get<0>(size_alloc_); } + StorageElement* begin() const { return data_; } + StorageElement* end() const { return begin() + size(); } + allocator_type& alloc() { return std::get<1>(size_alloc_); } + + private: + static bool UsingInlinedStorage(size_type n) { + return n <= inline_elements; + } + + StorageElement* InitializeData() { + if (UsingInlinedStorage(size())) { + InlinedStorage::AnnotateConstruct(size()); + return InlinedStorage::data(); + } else { + return reinterpret_cast<StorageElement*>( + AllocatorTraits::allocate(alloc(), size())); + } + } + + // Using std::tuple and not absl::CompressedTuple, as it has a lot of + // dependencies to other absl headers. + std::tuple<size_type, allocator_type> size_alloc_; + StorageElement* data_; + }; + + Storage storage_; +}; + +template <typename T, size_t N, typename A> +constexpr size_t FixedArray<T, N, A>::kInlineBytesDefault; + +template <typename T, size_t N, typename A> +constexpr typename FixedArray<T, N, A>::size_type + FixedArray<T, N, A>::inline_elements; } // namespace internal } // namespace ceres diff --git a/extern/ceres/include/ceres/internal/householder_vector.h b/extern/ceres/include/ceres/internal/householder_vector.h new file mode 100644 index 00000000000..55f68e526a0 --- /dev/null +++ b/extern/ceres/include/ceres/internal/householder_vector.h @@ -0,0 +1,88 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) + +#ifndef CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_ +#define CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_ + +#include "Eigen/Core" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Algorithm 5.1.1 from 'Matrix Computations' by Golub et al. (Johns Hopkins +// Studies in Mathematical Sciences) but using the nth element of the input +// vector as pivot instead of first. This computes the vector v with v(n) = 1 +// and beta such that H = I - beta * v * v^T is orthogonal and +// H * x = ||x||_2 * e_n. +// +// NOTE: Some versions of MSVC have trouble deducing the type of v if +// you do not specify all the template arguments explicitly. +template <typename XVectorType, typename Scalar, int N> +void ComputeHouseholderVector(const XVectorType& x, + Eigen::Matrix<Scalar, N, 1>* v, + Scalar* beta) { + CHECK(beta != nullptr); + CHECK(v != nullptr); + CHECK_GT(x.rows(), 1); + CHECK_EQ(x.rows(), v->rows()); + + Scalar sigma = x.head(x.rows() - 1).squaredNorm(); + *v = x; + (*v)(v->rows() - 1) = Scalar(1.0); + + *beta = Scalar(0.0); + const Scalar& x_pivot = x(x.rows() - 1); + + if (sigma <= Scalar(std::numeric_limits<double>::epsilon())) { + if (x_pivot < Scalar(0.0)) { + *beta = Scalar(2.0); + } + return; + } + + const Scalar mu = sqrt(x_pivot * x_pivot + sigma); + Scalar v_pivot = Scalar(1.0); + + if (x_pivot <= Scalar(0.0)) { + v_pivot = x_pivot - mu; + } else { + v_pivot = -sigma / (x_pivot + mu); + } + + *beta = Scalar(2.0) * v_pivot * v_pivot / (sigma + v_pivot * v_pivot); + + v->head(v->rows() - 1) /= v_pivot; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_ diff --git a/extern/ceres/include/ceres/internal/integer_sequence_algorithm.h b/extern/ceres/include/ceres/internal/integer_sequence_algorithm.h new file mode 100644 index 00000000000..170acac2832 --- /dev/null +++ b/extern/ceres/include/ceres/internal/integer_sequence_algorithm.h @@ -0,0 +1,165 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: jodebo_beck@gmx.de (Johannes Beck) +// +// Algorithms to be used together with integer_sequence, like computing the sum +// or the exclusive scan (sometimes called exclusive prefix sum) at compile +// time. + +#ifndef CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_ +#define CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_ + +#include <utility> + +namespace ceres { +namespace internal { + +// Implementation of calculating the sum of an integer sequence. +// Recursively instantiate SumImpl and calculate the sum of the N first +// numbers. This reduces the number of instantiations and speeds up +// compilation. +// +// Examples: +// 1) integer_sequence<int, 5>: +// Value = 5 +// +// 2) integer_sequence<int, 4, 2>: +// Value = 4 + 2 + SumImpl<integer_sequence<int>>::Value +// Value = 4 + 2 + 0 +// +// 3) integer_sequence<int, 2, 1, 4>: +// Value = 2 + 1 + SumImpl<integer_sequence<int, 4>>::Value +// Value = 2 + 1 + 4 +template <typename Seq> +struct SumImpl; + +// Strip of and sum the first number. +template <typename T, T N, T... Ns> +struct SumImpl<std::integer_sequence<T, N, Ns...>> { + static constexpr T Value = N + SumImpl<std::integer_sequence<T, Ns...>>::Value; +}; + +// Strip of and sum the first two numbers. +template <typename T, T N1, T N2, T... Ns> +struct SumImpl<std::integer_sequence<T, N1, N2, Ns...>> { + static constexpr T Value = + N1 + N2 + SumImpl<std::integer_sequence<T, Ns...>>::Value; +}; + +// Strip of and sum the first four numbers. +template <typename T, T N1, T N2, T N3, T N4, T... Ns> +struct SumImpl<std::integer_sequence<T, N1, N2, N3, N4, Ns...>> { + static constexpr T Value = + N1 + N2 + N3 + N4 + SumImpl<std::integer_sequence<T, Ns...>>::Value; +}; + +// Only one number is left. 'Value' is just that number ('recursion' ends). +template <typename T, T N> +struct SumImpl<std::integer_sequence<T, N>> { + static constexpr T Value = N; +}; + +// No number is left. 'Value' is the identity element (for sum this is zero). +template <typename T> +struct SumImpl<std::integer_sequence<T>> { + static constexpr T Value = T(0); +}; + +// Calculate the sum of an integer sequence. The resulting sum will be stored in +// 'Value'. +template <typename Seq> +class Sum { + using T = typename Seq::value_type; + + public: + static constexpr T Value = SumImpl<Seq>::Value; +}; + +// Implementation of calculating an exclusive scan (exclusive prefix sum) of an +// integer sequence. Exclusive means that the i-th input element is not included +// in the i-th sum. Calculating the exclusive scan for an input array I results +// in the following output R: +// +// R[0] = 0 +// R[1] = I[0]; +// R[2] = I[0] + I[1]; +// R[3] = I[0] + I[1] + I[2]; +// ... +// +// In C++17 std::exclusive_scan does the same operation at runtime (but +// cannot be used to calculate the prefix sum at compile time). See +// https://en.cppreference.com/w/cpp/algorithm/exclusive_scan for a more +// detailed description. +// +// Example for integer_sequence<int, 1, 4, 3> (seq := integer_sequence): +// T , Sum, Ns... , Rs... +// ExclusiveScanImpl<int, 0, seq<int, 1, 4, 3>, seq<int >> +// ExclusiveScanImpl<int, 1, seq<int, 4, 3>, seq<int, 0 >> +// ExclusiveScanImpl<int, 5, seq<int, 3>, seq<int, 0, 1 >> +// ExclusiveScanImpl<int, 8, seq<int >, seq<int, 0, 1, 5>> +// ^^^^^^^^^^^^^^^^^ +// resulting sequence +template <typename T, T Sum, typename SeqIn, typename SeqOut> +struct ExclusiveScanImpl; + +template <typename T, T Sum, T N, T... Ns, T... Rs> +struct ExclusiveScanImpl<T, Sum, std::integer_sequence<T, N, Ns...>, + std::integer_sequence<T, Rs...>> { + using Type = + typename ExclusiveScanImpl<T, Sum + N, std::integer_sequence<T, Ns...>, + std::integer_sequence<T, Rs..., Sum>>::Type; +}; + +// End of 'recursion'. The resulting type is SeqOut. +template <typename T, T Sum, typename SeqOut> +struct ExclusiveScanImpl<T, Sum, std::integer_sequence<T>, SeqOut> { + using Type = SeqOut; +}; + +// Calculates the exclusive scan of the specified integer sequence. The last +// element (the total) is not included in the resulting sequence so they have +// same length. This means the exclusive scan of integer_sequence<int, 1, 2, 3> +// will be integer_sequence<int, 0, 1, 3>. +template <typename Seq> +class ExclusiveScanT { + using T = typename Seq::value_type; + + public: + using Type = + typename ExclusiveScanImpl<T, T(0), Seq, std::integer_sequence<T>>::Type; +}; + +// Helper to use exclusive scan without typename. +template <typename Seq> +using ExclusiveScan = typename ExclusiveScanT<Seq>::Type; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_ diff --git a/extern/ceres/include/ceres/internal/line_parameterization.h b/extern/ceres/include/ceres/internal/line_parameterization.h new file mode 100644 index 00000000000..eda390148df --- /dev/null +++ b/extern/ceres/include/ceres/internal/line_parameterization.h @@ -0,0 +1,183 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2020 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: jodebo_beck@gmx.de (Johannes Beck) +// + +#ifndef CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_ +#define CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_ + +#include "householder_vector.h" + +namespace ceres { + +template <int AmbientSpaceDimension> +bool LineParameterization<AmbientSpaceDimension>::Plus( + const double* x_ptr, + const double* delta_ptr, + double* x_plus_delta_ptr) const { + // We seek a box plus operator of the form + // + // [o*, d*] = Plus([o, d], [delta_o, delta_d]) + // + // where o is the origin point, d is the direction vector, delta_o is + // the delta of the origin point and delta_d the delta of the direction and + // o* and d* is the updated origin point and direction. + // + // We separate the Plus operator into the origin point and directional part + // d* = Plus_d(d, delta_d) + // o* = Plus_o(o, d, delta_o) + // + // The direction update function Plus_d is the same as for the homogeneous + // vector parameterization: + // + // d* = H_{v(d)} [0.5 sinc(0.5 |delta_d|) delta_d, cos(0.5 |delta_d|)]^T + // + // where H is the householder matrix + // H_{v} = I - (2 / |v|^2) v v^T + // and + // v(d) = d - sign(d_n) |d| e_n. + // + // The origin point update function Plus_o is defined as + // + // o* = o + H_{v(d)} [0.5 delta_o, 0]^T. + + static constexpr int kDim = AmbientSpaceDimension; + using AmbientVector = Eigen::Matrix<double, kDim, 1>; + using AmbientVectorRef = Eigen::Map<Eigen::Matrix<double, kDim, 1>>; + using ConstAmbientVectorRef = + Eigen::Map<const Eigen::Matrix<double, kDim, 1>>; + using ConstTangentVectorRef = + Eigen::Map<const Eigen::Matrix<double, kDim - 1, 1>>; + + ConstAmbientVectorRef o(x_ptr); + ConstAmbientVectorRef d(x_ptr + kDim); + + ConstTangentVectorRef delta_o(delta_ptr); + ConstTangentVectorRef delta_d(delta_ptr + kDim - 1); + AmbientVectorRef o_plus_delta(x_plus_delta_ptr); + AmbientVectorRef d_plus_delta(x_plus_delta_ptr + kDim); + + const double norm_delta_d = delta_d.norm(); + + o_plus_delta = o; + + // Shortcut for zero delta direction. + if (norm_delta_d == 0.0) { + d_plus_delta = d; + + if (delta_o.isZero(0.0)) { + return true; + } + } + + // Calculate the householder transformation which is needed for f_d and f_o. + AmbientVector v; + double beta; + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + internal::ComputeHouseholderVector<ConstAmbientVectorRef, double, kDim>( + d, &v, &beta); + + if (norm_delta_d != 0.0) { + // Map the delta from the minimum representation to the over parameterized + // homogeneous vector. See section A6.9.2 on page 624 of Hartley & Zisserman + // (2nd Edition) for a detailed description. Note there is a typo on Page + // 625, line 4 so check the book errata. + const double norm_delta_div_2 = 0.5 * norm_delta_d; + const double sin_delta_by_delta = + std::sin(norm_delta_div_2) / norm_delta_div_2; + + // Apply the delta update to remain on the unit sphere. See section A6.9.3 + // on page 625 of Hartley & Zisserman (2nd Edition) for a detailed + // description. + AmbientVector y; + y.template head<kDim - 1>() = 0.5 * sin_delta_by_delta * delta_d; + y[kDim - 1] = std::cos(norm_delta_div_2); + + d_plus_delta = d.norm() * (y - v * (beta * (v.transpose() * y))); + } + + // The null space is in the direction of the line, so the tangent space is + // perpendicular to the line direction. This is achieved by using the + // householder matrix of the direction and allow only movements + // perpendicular to e_n. + // + // The factor of 0.5 is used to be consistent with the line direction + // update. + AmbientVector y; + y << 0.5 * delta_o, 0; + o_plus_delta += y - v * (beta * (v.transpose() * y)); + + return true; +} + +template <int AmbientSpaceDimension> +bool LineParameterization<AmbientSpaceDimension>::ComputeJacobian( + const double* x_ptr, double* jacobian_ptr) const { + static constexpr int kDim = AmbientSpaceDimension; + using AmbientVector = Eigen::Matrix<double, kDim, 1>; + using ConstAmbientVectorRef = + Eigen::Map<const Eigen::Matrix<double, kDim, 1>>; + using MatrixRef = Eigen::Map< + Eigen::Matrix<double, 2 * kDim, 2 * (kDim - 1), Eigen::RowMajor>>; + + ConstAmbientVectorRef d(x_ptr + kDim); + MatrixRef jacobian(jacobian_ptr); + + // Clear the Jacobian as only half of the matrix is not zero. + jacobian.setZero(); + + AmbientVector v; + double beta; + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + internal::ComputeHouseholderVector<ConstAmbientVectorRef, double, kDim>( + d, &v, &beta); + + // The Jacobian is equal to J = 0.5 * H.leftCols(kDim - 1) where H is + // the Householder matrix (H = I - beta * v * v') for the origin point. For + // the line direction part the Jacobian is scaled by the norm of the + // direction. + for (int i = 0; i < kDim - 1; ++i) { + jacobian.block(0, i, kDim, 1) = -0.5 * beta * v(i) * v; + jacobian.col(i)(i) += 0.5; + } + + jacobian.template block<kDim, kDim - 1>(kDim, kDim - 1) = + jacobian.template block<kDim, kDim - 1>(0, 0) * d.norm(); + return true; +} + +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_ diff --git a/extern/ceres/include/ceres/internal/macros.h b/extern/ceres/include/ceres/internal/macros.h deleted file mode 100644 index bebb965e25b..00000000000 --- a/extern/ceres/include/ceres/internal/macros.h +++ /dev/null @@ -1,170 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://ceres-solver.org/ -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// * Neither the name of Google Inc. nor the names of its contributors may be -// used to endorse or promote products derived from this software without -// specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// -// Various Google-specific macros. -// -// This code is compiled directly on many platforms, including client -// platforms like Windows, Mac, and embedded systems. Before making -// any changes here, make sure that you're not breaking any platforms. - -#ifndef CERES_PUBLIC_INTERNAL_MACROS_H_ -#define CERES_PUBLIC_INTERNAL_MACROS_H_ - -#include <cstddef> // For size_t. - -// A macro to disallow the copy constructor and operator= functions -// This should be used in the private: declarations for a class -// -// For disallowing only assign or copy, write the code directly, but declare -// the intend in a comment, for example: -// -// void operator=(const TypeName&); // _DISALLOW_ASSIGN - -// Note, that most uses of CERES_DISALLOW_ASSIGN and CERES_DISALLOW_COPY -// are broken semantically, one should either use disallow both or -// neither. Try to avoid these in new code. -#define CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) \ - TypeName(const TypeName&); \ - void operator=(const TypeName&) - -// A macro to disallow all the implicit constructors, namely the -// default constructor, copy constructor and operator= functions. -// -// This should be used in the private: declarations for a class -// that wants to prevent anyone from instantiating it. This is -// especially useful for classes containing only static methods. -#define CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(TypeName) \ - TypeName(); \ - CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) - -// The arraysize(arr) macro returns the # of elements in an array arr. -// The expression is a compile-time constant, and therefore can be -// used in defining new arrays, for example. If you use arraysize on -// a pointer by mistake, you will get a compile-time error. -// -// One caveat is that arraysize() doesn't accept any array of an -// anonymous type or a type defined inside a function. In these rare -// cases, you have to use the unsafe ARRAYSIZE() macro below. This is -// due to a limitation in C++'s template system. The limitation might -// eventually be removed, but it hasn't happened yet. - -// This template function declaration is used in defining arraysize. -// Note that the function doesn't need an implementation, as we only -// use its type. -template <typename T, size_t N> -char (&ArraySizeHelper(T (&array)[N]))[N]; - -// That gcc wants both of these prototypes seems mysterious. VC, for -// its part, can't decide which to use (another mystery). Matching of -// template overloads: the final frontier. -#ifndef _WIN32 -template <typename T, size_t N> -char (&ArraySizeHelper(const T (&array)[N]))[N]; -#endif - -#define arraysize(array) (sizeof(ArraySizeHelper(array))) - -// ARRAYSIZE performs essentially the same calculation as arraysize, -// but can be used on anonymous types or types defined inside -// functions. It's less safe than arraysize as it accepts some -// (although not all) pointers. Therefore, you should use arraysize -// whenever possible. -// -// The expression ARRAYSIZE(a) is a compile-time constant of type -// size_t. -// -// ARRAYSIZE catches a few type errors. If you see a compiler error -// -// "warning: division by zero in ..." -// -// when using ARRAYSIZE, you are (wrongfully) giving it a pointer. -// You should only use ARRAYSIZE on statically allocated arrays. -// -// The following comments are on the implementation details, and can -// be ignored by the users. -// -// ARRAYSIZE(arr) works by inspecting sizeof(arr) (the # of bytes in -// the array) and sizeof(*(arr)) (the # of bytes in one array -// element). If the former is divisible by the latter, perhaps arr is -// indeed an array, in which case the division result is the # of -// elements in the array. Otherwise, arr cannot possibly be an array, -// and we generate a compiler error to prevent the code from -// compiling. -// -// Since the size of bool is implementation-defined, we need to cast -// !(sizeof(a) & sizeof(*(a))) to size_t in order to ensure the final -// result has type size_t. -// -// This macro is not perfect as it wrongfully accepts certain -// pointers, namely where the pointer size is divisible by the pointee -// size. Since all our code has to go through a 32-bit compiler, -// where a pointer is 4 bytes, this means all pointers to a type whose -// size is 3 or greater than 4 will be (righteously) rejected. -// -// Kudos to Jorg Brown for this simple and elegant implementation. -// -// - wan 2005-11-16 -// -// Starting with Visual C++ 2005, WinNT.h includes ARRAYSIZE. However, -// the definition comes from the over-broad windows.h header that -// introduces a macro, ERROR, that conflicts with the logging framework -// that Ceres uses. Instead, rename ARRAYSIZE to CERES_ARRAYSIZE. -#define CERES_ARRAYSIZE(a) \ - ((sizeof(a) / sizeof(*(a))) / \ - static_cast<size_t>(!(sizeof(a) % sizeof(*(a))))) - -// Tell the compiler to warn about unused return values for functions -// declared with this macro. The macro should be used on function -// declarations following the argument list: -// -// Sprocket* AllocateSprocket() MUST_USE_RESULT; -// -#if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) \ - && !defined(COMPILER_ICC) -#define CERES_MUST_USE_RESULT __attribute__ ((warn_unused_result)) -#else -#define CERES_MUST_USE_RESULT -#endif - -// Platform independent macros to get aligned memory allocations. -// For example -// -// MyFoo my_foo CERES_ALIGN_ATTRIBUTE(16); -// -// Gives us an instance of MyFoo which is aligned at a 16 byte -// boundary. -#if defined(_MSC_VER) -#define CERES_ALIGN_ATTRIBUTE(n) __declspec(align(n)) -#define CERES_ALIGN_OF(T) __alignof(T) -#elif defined(__GNUC__) -#define CERES_ALIGN_ATTRIBUTE(n) __attribute__((aligned(n))) -#define CERES_ALIGN_OF(T) __alignof(T) -#endif - -#endif // CERES_PUBLIC_INTERNAL_MACROS_H_ diff --git a/extern/ceres/include/ceres/internal/manual_constructor.h b/extern/ceres/include/ceres/internal/manual_constructor.h deleted file mode 100644 index 0d7633cef83..00000000000 --- a/extern/ceres/include/ceres/internal/manual_constructor.h +++ /dev/null @@ -1,208 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://ceres-solver.org/ -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// * Neither the name of Google Inc. nor the names of its contributors may be -// used to endorse or promote products derived from this software without -// specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: kenton@google.com (Kenton Varda) -// -// ManualConstructor statically-allocates space in which to store some -// object, but does not initialize it. You can then call the constructor -// and destructor for the object yourself as you see fit. This is useful -// for memory management optimizations, where you want to initialize and -// destroy an object multiple times but only allocate it once. -// -// (When I say ManualConstructor statically allocates space, I mean that -// the ManualConstructor object itself is forced to be the right size.) - -#ifndef CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ -#define CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ - -#include <new> - -namespace ceres { -namespace internal { - -// ------- Define CERES_ALIGNED_CHAR_ARRAY -------------------------------- - -#ifndef CERES_ALIGNED_CHAR_ARRAY - -// Because MSVC and older GCCs require that the argument to their alignment -// construct to be a literal constant integer, we use a template instantiated -// at all the possible powers of two. -template<int alignment, int size> struct AlignType { }; -template<int size> struct AlignType<0, size> { typedef char result[size]; }; - -#if !defined(CERES_ALIGN_ATTRIBUTE) -#define CERES_ALIGNED_CHAR_ARRAY you_must_define_CERES_ALIGNED_CHAR_ARRAY_for_your_compiler -#else // !defined(CERES_ALIGN_ATTRIBUTE) - -#define CERES_ALIGN_TYPE_TEMPLATE(X) \ - template<int size> struct AlignType<X, size> { \ - typedef CERES_ALIGN_ATTRIBUTE(X) char result[size]; \ - } - -CERES_ALIGN_TYPE_TEMPLATE(1); -CERES_ALIGN_TYPE_TEMPLATE(2); -CERES_ALIGN_TYPE_TEMPLATE(4); -CERES_ALIGN_TYPE_TEMPLATE(8); -CERES_ALIGN_TYPE_TEMPLATE(16); -CERES_ALIGN_TYPE_TEMPLATE(32); -CERES_ALIGN_TYPE_TEMPLATE(64); -CERES_ALIGN_TYPE_TEMPLATE(128); -CERES_ALIGN_TYPE_TEMPLATE(256); -CERES_ALIGN_TYPE_TEMPLATE(512); -CERES_ALIGN_TYPE_TEMPLATE(1024); -CERES_ALIGN_TYPE_TEMPLATE(2048); -CERES_ALIGN_TYPE_TEMPLATE(4096); -CERES_ALIGN_TYPE_TEMPLATE(8192); -// Any larger and MSVC++ will complain. - -#undef CERES_ALIGN_TYPE_TEMPLATE - -#define CERES_ALIGNED_CHAR_ARRAY(T, Size) \ - typename AlignType<CERES_ALIGN_OF(T), sizeof(T) * Size>::result - -#endif // !defined(CERES_ALIGN_ATTRIBUTE) - -#endif // CERES_ALIGNED_CHAR_ARRAY - -template <typename Type> -class ManualConstructor { - public: - // No constructor or destructor because one of the most useful uses of - // this class is as part of a union, and members of a union cannot have - // constructors or destructors. And, anyway, the whole point of this - // class is to bypass these. - - inline Type* get() { - return reinterpret_cast<Type*>(space_); - } - inline const Type* get() const { - return reinterpret_cast<const Type*>(space_); - } - - inline Type* operator->() { return get(); } - inline const Type* operator->() const { return get(); } - - inline Type& operator*() { return *get(); } - inline const Type& operator*() const { return *get(); } - - // This is needed to get around the strict aliasing warning GCC generates. - inline void* space() { - return reinterpret_cast<void*>(space_); - } - - // You can pass up to four constructor arguments as arguments of Init(). - inline void Init() { - new(space()) Type; - } - - template <typename T1> - inline void Init(const T1& p1) { - new(space()) Type(p1); - } - - template <typename T1, typename T2> - inline void Init(const T1& p1, const T2& p2) { - new(space()) Type(p1, p2); - } - - template <typename T1, typename T2, typename T3> - inline void Init(const T1& p1, const T2& p2, const T3& p3) { - new(space()) Type(p1, p2, p3); - } - - template <typename T1, typename T2, typename T3, typename T4> - inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4) { - new(space()) Type(p1, p2, p3, p4); - } - - template <typename T1, typename T2, typename T3, typename T4, typename T5> - inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, - const T5& p5) { - new(space()) Type(p1, p2, p3, p4, p5); - } - - template <typename T1, typename T2, typename T3, typename T4, typename T5, - typename T6> - inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, - const T5& p5, const T6& p6) { - new(space()) Type(p1, p2, p3, p4, p5, p6); - } - - template <typename T1, typename T2, typename T3, typename T4, typename T5, - typename T6, typename T7> - inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, - const T5& p5, const T6& p6, const T7& p7) { - new(space()) Type(p1, p2, p3, p4, p5, p6, p7); - } - - template <typename T1, typename T2, typename T3, typename T4, typename T5, - typename T6, typename T7, typename T8> - inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, - const T5& p5, const T6& p6, const T7& p7, const T8& p8) { - new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8); - } - - template <typename T1, typename T2, typename T3, typename T4, typename T5, - typename T6, typename T7, typename T8, typename T9> - inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, - const T5& p5, const T6& p6, const T7& p7, const T8& p8, - const T9& p9) { - new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9); - } - - template <typename T1, typename T2, typename T3, typename T4, typename T5, - typename T6, typename T7, typename T8, typename T9, typename T10> - inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, - const T5& p5, const T6& p6, const T7& p7, const T8& p8, - const T9& p9, const T10& p10) { - new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10); - } - - template <typename T1, typename T2, typename T3, typename T4, typename T5, - typename T6, typename T7, typename T8, typename T9, typename T10, - typename T11> - inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, - const T5& p5, const T6& p6, const T7& p7, const T8& p8, - const T9& p9, const T10& p10, const T11& p11) { - new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11); - } - - inline void Destroy() { - get()->~Type(); - } - - private: - CERES_ALIGNED_CHAR_ARRAY(Type, 1) space_; -}; - -#undef CERES_ALIGNED_CHAR_ARRAY - -} // namespace internal -} // namespace ceres - -#endif // CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ diff --git a/extern/ceres/include/ceres/internal/memory.h b/extern/ceres/include/ceres/internal/memory.h new file mode 100644 index 00000000000..45c5b67c353 --- /dev/null +++ b/extern/ceres/include/ceres/internal/memory.h @@ -0,0 +1,90 @@ +// Copyright 2017 The Abseil Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// https://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// ----------------------------------------------------------------------------- +// File: memory.h +// ----------------------------------------------------------------------------- +// +// This header file contains utility functions for managing the creation and +// conversion of smart pointers. This file is an extension to the C++ +// standard <memory> library header file. + +#ifndef CERES_PUBLIC_INTERNAL_MEMORY_H_ +#define CERES_PUBLIC_INTERNAL_MEMORY_H_ + +#include <memory> + +#ifdef CERES_HAVE_EXCEPTIONS +#define CERES_INTERNAL_TRY try +#define CERES_INTERNAL_CATCH_ANY catch (...) +#define CERES_INTERNAL_RETHROW \ + do { \ + throw; \ + } while (false) +#else // CERES_HAVE_EXCEPTIONS +#define CERES_INTERNAL_TRY if (true) +#define CERES_INTERNAL_CATCH_ANY else if (false) +#define CERES_INTERNAL_RETHROW \ + do { \ + } while (false) +#endif // CERES_HAVE_EXCEPTIONS + +namespace ceres { +namespace internal { + +template <typename Allocator, typename Iterator, typename... Args> +void ConstructRange(Allocator& alloc, + Iterator first, + Iterator last, + const Args&... args) { + for (Iterator cur = first; cur != last; ++cur) { + CERES_INTERNAL_TRY { + std::allocator_traits<Allocator>::construct( + alloc, std::addressof(*cur), args...); + } + CERES_INTERNAL_CATCH_ANY { + while (cur != first) { + --cur; + std::allocator_traits<Allocator>::destroy(alloc, std::addressof(*cur)); + } + CERES_INTERNAL_RETHROW; + } + } +} + +template <typename Allocator, typename Iterator, typename InputIterator> +void CopyRange(Allocator& alloc, + Iterator destination, + InputIterator first, + InputIterator last) { + for (Iterator cur = destination; first != last; + static_cast<void>(++cur), static_cast<void>(++first)) { + CERES_INTERNAL_TRY { + std::allocator_traits<Allocator>::construct( + alloc, std::addressof(*cur), *first); + } + CERES_INTERNAL_CATCH_ANY { + while (cur != destination) { + --cur; + std::allocator_traits<Allocator>::destroy(alloc, std::addressof(*cur)); + } + CERES_INTERNAL_RETHROW; + } + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_MEMORY_H_ diff --git a/extern/ceres/include/ceres/internal/numeric_diff.h b/extern/ceres/include/ceres/internal/numeric_diff.h index 11e8275b1d3..fb2e00baca5 100644 --- a/extern/ceres/include/ceres/internal/numeric_diff.h +++ b/extern/ceres/include/ceres/internal/numeric_diff.h @@ -36,12 +36,12 @@ #define CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ #include <cstring> +#include <utility> #include "Eigen/Dense" #include "Eigen/StdVector" #include "ceres/cost_function.h" #include "ceres/internal/fixed_array.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/internal/variadic_evaluate.h" #include "ceres/numeric_diff_options.h" #include "ceres/types.h" @@ -51,42 +51,11 @@ namespace ceres { namespace internal { -// Helper templates that allow evaluation of a variadic functor or a -// CostFunction object. -template <typename CostFunctor, - int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8, int N9 > -bool EvaluateImpl(const CostFunctor* functor, - double const* const* parameters, - double* residuals, - const void* /* NOT USED */) { - return VariadicEvaluate<CostFunctor, - double, - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call( - *functor, - parameters, - residuals); -} - -template <typename CostFunctor, - int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8, int N9 > -bool EvaluateImpl(const CostFunctor* functor, - double const* const* parameters, - double* residuals, - const CostFunction* /* NOT USED */) { - return functor->Evaluate(parameters, residuals, NULL); -} - // This is split from the main class because C++ doesn't allow partial template // specializations for member functions. The alternative is to repeat the main // class for differing numbers of parameters, which is also unfortunate. -template <typename CostFunctor, - NumericDiffMethodType kMethod, - int kNumResiduals, - int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8, int N9, - int kParameterBlock, +template <typename CostFunctor, NumericDiffMethodType kMethod, + int kNumResiduals, typename ParameterDims, int kParameterBlock, int kParameterBlockSize> struct NumericDiff { // Mutates parameters but must restore them before return. @@ -104,6 +73,8 @@ struct NumericDiff { using Eigen::RowMajor; using Eigen::ColMajor; + DCHECK(jacobian); + const int num_residuals_internal = (kNumResiduals != ceres::DYNAMIC ? kNumResiduals : num_residuals); const int parameter_block_index_internal = @@ -155,7 +126,7 @@ struct NumericDiff { // compute the derivative for that parameter. FixedArray<double> temp_residual_array(num_residuals_internal); FixedArray<double> residual_array(num_residuals_internal); - Map<ResidualVector> residuals(residual_array.get(), + Map<ResidualVector> residuals(residual_array.data(), num_residuals_internal); for (int j = 0; j < parameter_block_size_internal; ++j) { @@ -170,8 +141,8 @@ struct NumericDiff { residuals_at_eval_point, parameters, x_plus_delta.data(), - temp_residual_array.get(), - residual_array.get())) { + temp_residual_array.data(), + residual_array.data())) { return false; } } else { @@ -182,8 +153,8 @@ struct NumericDiff { residuals_at_eval_point, parameters, x_plus_delta.data(), - temp_residual_array.get(), - residual_array.get())) { + temp_residual_array.data(), + residual_array.data())) { return false; } } @@ -220,8 +191,9 @@ struct NumericDiff { // Mutate 1 element at a time and then restore. x_plus_delta(parameter_index) = x(parameter_index) + delta; - if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( - functor, parameters, residuals.data(), functor)) { + if (!VariadicEvaluate<ParameterDims>(*functor, + parameters, + residuals.data())) { return false; } @@ -234,8 +206,9 @@ struct NumericDiff { // Compute the function on the other side of x(parameter_index). x_plus_delta(parameter_index) = x(parameter_index) - delta; - if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( - functor, parameters, temp_residuals.data(), functor)) { + if (!VariadicEvaluate<ParameterDims>(*functor, + parameters, + temp_residuals.data())) { return false; } @@ -407,35 +380,116 @@ struct NumericDiff { } }; -template <typename CostFunctor, - NumericDiffMethodType kMethod, - int kNumResiduals, - int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8, int N9, - int kParameterBlock> -struct NumericDiff<CostFunctor, kMethod, kNumResiduals, - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9, - kParameterBlock, 0> { - // Mutates parameters but must restore them before return. - static bool EvaluateJacobianForParameterBlock( - const CostFunctor* functor, - const double* residuals_at_eval_point, - const NumericDiffOptions& options, - const int num_residuals, - const int parameter_block_index, - const int parameter_block_size, - double **parameters, - double *jacobian) { - // Silence unused parameter compiler warnings. - (void)functor; - (void)residuals_at_eval_point; - (void)options; - (void)num_residuals; - (void)parameter_block_index; - (void)parameter_block_size; - (void)parameters; - (void)jacobian; - LOG(FATAL) << "Control should never reach here."; +// This function calls NumericDiff<...>::EvaluateJacobianForParameterBlock for +// each parameter block. +// +// Example: +// A call to +// EvaluateJacobianForParameterBlocks<StaticParameterDims<2, 3>>( +// functor, +// residuals_at_eval_point, +// options, +// num_residuals, +// parameters, +// jacobians); +// will result in the following calls to +// NumericDiff<...>::EvaluateJacobianForParameterBlock: +// +// if (jacobians[0] != nullptr) { +// if (!NumericDiff< +// CostFunctor, +// method, +// kNumResiduals, +// StaticParameterDims<2, 3>, +// 0, +// 2>::EvaluateJacobianForParameterBlock(functor, +// residuals_at_eval_point, +// options, +// num_residuals, +// 0, +// 2, +// parameters, +// jacobians[0])) { +// return false; +// } +// } +// if (jacobians[1] != nullptr) { +// if (!NumericDiff< +// CostFunctor, +// method, +// kNumResiduals, +// StaticParameterDims<2, 3>, +// 1, +// 3>::EvaluateJacobianForParameterBlock(functor, +// residuals_at_eval_point, +// options, +// num_residuals, +// 1, +// 3, +// parameters, +// jacobians[1])) { +// return false; +// } +// } +template <typename ParameterDims, + typename Parameters = typename ParameterDims::Parameters, + int ParameterIdx = 0> +struct EvaluateJacobianForParameterBlocks; + +template <typename ParameterDims, int N, int... Ns, int ParameterIdx> +struct EvaluateJacobianForParameterBlocks<ParameterDims, + std::integer_sequence<int, N, Ns...>, + ParameterIdx> { + template <NumericDiffMethodType method, + int kNumResiduals, + typename CostFunctor> + static bool Apply(const CostFunctor* functor, + const double* residuals_at_eval_point, + const NumericDiffOptions& options, + int num_residuals, + double** parameters, + double** jacobians) { + if (jacobians[ParameterIdx] != nullptr) { + if (!NumericDiff< + CostFunctor, + method, + kNumResiduals, + ParameterDims, + ParameterIdx, + N>::EvaluateJacobianForParameterBlock(functor, + residuals_at_eval_point, + options, + num_residuals, + ParameterIdx, + N, + parameters, + jacobians[ParameterIdx])) { + return false; + } + } + + return EvaluateJacobianForParameterBlocks<ParameterDims, + std::integer_sequence<int, Ns...>, + ParameterIdx + 1>:: + template Apply<method, kNumResiduals>(functor, + residuals_at_eval_point, + options, + num_residuals, + parameters, + jacobians); + } +}; + +// End of 'recursion'. Nothing more to do. +template <typename ParameterDims, int ParameterIdx> +struct EvaluateJacobianForParameterBlocks<ParameterDims, std::integer_sequence<int>, + ParameterIdx> { + template <NumericDiffMethodType method, int kNumResiduals, + typename CostFunctor> + static bool Apply(const CostFunctor* /* NOT USED*/, + const double* /* NOT USED*/, + const NumericDiffOptions& /* NOT USED*/, int /* NOT USED*/, + double** /* NOT USED*/, double** /* NOT USED*/) { return true; } }; diff --git a/extern/ceres/include/ceres/internal/parameter_dims.h b/extern/ceres/include/ceres/internal/parameter_dims.h new file mode 100644 index 00000000000..24021061416 --- /dev/null +++ b/extern/ceres/include/ceres/internal/parameter_dims.h @@ -0,0 +1,124 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: jodebo_beck@gmx.de (Johannes Beck) + +#ifndef CERES_PUBLIC_INTERNAL_PARAMETER_DIMS_H_ +#define CERES_PUBLIC_INTERNAL_PARAMETER_DIMS_H_ + +#include <array> +#include <utility> + +#include "ceres/internal/integer_sequence_algorithm.h" + +namespace ceres { +namespace internal { + +// Checks, whether the given parameter block sizes are valid. Valid means every +// dimension is bigger than zero. +constexpr bool IsValidParameterDimensionSequence(std::integer_sequence<int>) { + return true; +} + +template <int N, int... Ts> +constexpr bool IsValidParameterDimensionSequence( + std::integer_sequence<int, N, Ts...>) { + return (N <= 0) ? false + : IsValidParameterDimensionSequence( + std::integer_sequence<int, Ts...>()); +} + +// Helper class that represents the parameter dimensions. The parameter +// dimensions are either dynamic or the sizes are known at compile time. It is +// used to pass parameter block dimensions around (e.g. between functions or +// classes). +// +// As an example if one have three parameter blocks with dimensions (2, 4, 1), +// one would use 'StaticParameterDims<2, 4, 1>' which is a synonym for +// 'ParameterDims<false, 2, 4, 1>'. +// For dynamic parameter dims, one would just use 'DynamicParameterDims', which +// is a synonym for 'ParameterDims<true>'. +template <bool IsDynamic, int... Ns> +class ParameterDims { + public: + using Parameters = std::integer_sequence<int, Ns...>; + + // The parameter dimensions are only valid if all parameter block dimensions + // are greater than zero. + static constexpr bool kIsValid = + IsValidParameterDimensionSequence(Parameters()); + static_assert(kIsValid, + "Invalid parameter block dimension detected. Each parameter " + "block dimension must be bigger than zero."); + + static constexpr bool kIsDynamic = IsDynamic; + static constexpr int kNumParameterBlocks = sizeof...(Ns); + static_assert(kIsDynamic || kNumParameterBlocks > 0, + "At least one parameter block must be specified."); + + static constexpr int kNumParameters = + Sum<std::integer_sequence<int, Ns...>>::Value; + + static constexpr int GetDim(int dim) { return params_[dim]; } + + // If one has all parameters packed into a single array this function unpacks + // the parameters. + template <typename T> + static inline std::array<T*, kNumParameterBlocks> GetUnpackedParameters( + T* ptr) { + using Offsets = ExclusiveScan<Parameters>; + return GetUnpackedParameters(ptr, Offsets()); + } + + private: + template <typename T, int... Indices> + static inline std::array<T*, kNumParameterBlocks> GetUnpackedParameters( + T* ptr, std::integer_sequence<int, Indices...>) { + return std::array<T*, kNumParameterBlocks>{{ptr + Indices...}}; + } + + static constexpr std::array<int, kNumParameterBlocks> params_{Ns...}; +}; + +// Even static constexpr member variables needs to be defined (not only +// declared). As the ParameterDims class is tempalted this definition must +// be in the header file. +template <bool IsDynamic, int... Ns> +constexpr std::array<int, ParameterDims<IsDynamic, Ns...>::kNumParameterBlocks> + ParameterDims<IsDynamic, Ns...>::params_; + +// Using declarations for static and dynamic parameter dims. This makes client +// code easier to read. +template <int... Ns> +using StaticParameterDims = ParameterDims<false, Ns...>; +using DynamicParameterDims = ParameterDims<true>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_PARAMETER_DIMS_H_ diff --git a/extern/ceres/include/ceres/internal/port.h b/extern/ceres/include/ceres/internal/port.h index f4dcaee7bd8..958b0d15cb7 100644 --- a/extern/ceres/include/ceres/internal/port.h +++ b/extern/ceres/include/ceres/internal/port.h @@ -32,45 +32,41 @@ #define CERES_PUBLIC_INTERNAL_PORT_H_ // 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 -#include <memory> -#endif -namespace ceres { - -#if defined(CERES_TR1_SHARED_PTR) -using std::tr1::shared_ptr; +#if defined(CERES_USE_OPENMP) +# if defined(CERES_USE_CXX_THREADS) || defined(CERES_NO_THREADS) +# error CERES_USE_OPENMP is mutually exclusive to CERES_USE_CXX_THREADS and CERES_NO_THREADS +# endif +#elif defined(CERES_USE_CXX_THREADS) +# if defined(CERES_USE_OPENMP) || defined(CERES_NO_THREADS) +# error CERES_USE_CXX_THREADS is mutually exclusive to CERES_USE_OPENMP, CERES_USE_CXX_THREADS and CERES_NO_THREADS +# endif +#elif defined(CERES_NO_THREADS) +# if defined(CERES_USE_OPENMP) || defined(CERES_USE_CXX_THREADS) +# error CERES_NO_THREADS is mutually exclusive to CERES_USE_OPENMP and CERES_USE_CXX_THREADS +# endif #else -using std::shared_ptr; +# error One of CERES_USE_OPENMP, CERES_USE_CXX_THREADS or CERES_NO_THREADS must be defined. #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); +// CERES_NO_SPARSE should be automatically defined by config.h if Ceres was +// compiled without any sparse back-end. Verify that it has not subsequently +// been inconsistently redefined. +#if defined(CERES_NO_SPARSE) +# if !defined(CERES_NO_SUITESPARSE) +# error CERES_NO_SPARSE requires CERES_NO_SUITESPARSE. +# endif +# if !defined(CERES_NO_CXSPARSE) +# error CERES_NO_SPARSE requires CERES_NO_CXSPARSE +# endif +# if !defined(CERES_NO_ACCELERATE_SPARSE) +# error CERES_NO_SPARSE requires CERES_NO_ACCELERATE_SPARSE +# endif +# if defined(CERES_USE_EIGEN_SPARSE) +# error CERES_NO_SPARSE requires !CERES_USE_EIGEN_SPARSE +# endif #endif -} // namespace port_constants -#endif - -} // namespace ceres - -#endif // __cplusplus // A macro to signal which functions and classes are exported when // building a DLL with MSVC. diff --git a/extern/ceres/include/ceres/internal/scoped_ptr.h b/extern/ceres/include/ceres/internal/scoped_ptr.h deleted file mode 100644 index fa0ac25a031..00000000000 --- a/extern/ceres/include/ceres/internal/scoped_ptr.h +++ /dev/null @@ -1,310 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://ceres-solver.org/ -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// * Neither the name of Google Inc. nor the names of its contributors may be -// used to endorse or promote products derived from this software without -// specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: jorg@google.com (Jorg Brown) -// -// This is an implementation designed to match the anticipated future TR2 -// implementation of the scoped_ptr class, and its closely-related brethren, -// scoped_array, scoped_ptr_malloc, and make_scoped_ptr. - -#ifndef CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ -#define CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ - -#include <assert.h> -#include <stdlib.h> -#include <cstddef> -#include <algorithm> - -namespace ceres { -namespace internal { - -template <class C> class scoped_ptr; -template <class C, class Free> class scoped_ptr_malloc; -template <class C> class scoped_array; - -template <class C> -scoped_ptr<C> make_scoped_ptr(C *); - -// A scoped_ptr<T> is like a T*, except that the destructor of -// scoped_ptr<T> automatically deletes the pointer it holds (if -// any). That is, scoped_ptr<T> owns the T object that it points -// to. Like a T*, a scoped_ptr<T> may hold either NULL or a pointer to -// a T object. Also like T*, scoped_ptr<T> is thread-compatible, and -// once you dereference it, you get the threadsafety guarantees of T. -// -// The size of a scoped_ptr is small: sizeof(scoped_ptr<C>) == sizeof(C*) -template <class C> -class scoped_ptr { - public: - // The element type - typedef C element_type; - - // Constructor. Defaults to intializing with NULL. - // There is no way to create an uninitialized scoped_ptr. - // The input parameter must be allocated with new. - explicit scoped_ptr(C* p = NULL) : ptr_(p) { } - - // Destructor. If there is a C object, delete it. - // We don't need to test ptr_ == NULL because C++ does that for us. - ~scoped_ptr() { - enum { type_must_be_complete = sizeof(C) }; - delete ptr_; - } - - // Reset. Deletes the current owned object, if any. - // Then takes ownership of a new object, if given. - // this->reset(this->get()) works. - void reset(C* p = NULL) { - if (p != ptr_) { - enum { type_must_be_complete = sizeof(C) }; - delete ptr_; - ptr_ = p; - } - } - - // Accessors to get the owned object. - // operator* and operator-> will assert() if there is no current object. - C& operator*() const { - assert(ptr_ != NULL); - return *ptr_; - } - C* operator->() const { - assert(ptr_ != NULL); - return ptr_; - } - C* get() const { return ptr_; } - - // Comparison operators. - // These return whether a scoped_ptr and a raw pointer refer to - // the same object, not just to two different but equal objects. - bool operator==(const C* p) const { return ptr_ == p; } - bool operator!=(const C* p) const { return ptr_ != p; } - - // Swap two scoped pointers. - void swap(scoped_ptr& p2) { - C* tmp = ptr_; - ptr_ = p2.ptr_; - p2.ptr_ = tmp; - } - - // Release a pointer. - // The return value is the current pointer held by this object. - // If this object holds a NULL pointer, the return value is NULL. - // After this operation, this object will hold a NULL pointer, - // and will not own the object any more. - C* release() { - C* retVal = ptr_; - ptr_ = NULL; - return retVal; - } - - private: - C* ptr_; - - // google3 friend class that can access copy ctor (although if it actually - // calls a copy ctor, there will be a problem) see below - friend scoped_ptr<C> make_scoped_ptr<C>(C *p); - - // Forbid comparison of scoped_ptr types. If C2 != C, it totally doesn't - // make sense, and if C2 == C, it still doesn't make sense because you should - // never have the same object owned by two different scoped_ptrs. - template <class C2> bool operator==(scoped_ptr<C2> const& p2) const; - template <class C2> bool operator!=(scoped_ptr<C2> const& p2) const; - - // Disallow evil constructors - scoped_ptr(const scoped_ptr&); - void operator=(const scoped_ptr&); -}; - -// Free functions -template <class C> -inline void swap(scoped_ptr<C>& p1, scoped_ptr<C>& p2) { - p1.swap(p2); -} - -template <class C> -inline bool operator==(const C* p1, const scoped_ptr<C>& p2) { - return p1 == p2.get(); -} - -template <class C> -inline bool operator==(const C* p1, const scoped_ptr<const C>& p2) { - return p1 == p2.get(); -} - -template <class C> -inline bool operator!=(const C* p1, const scoped_ptr<C>& p2) { - return p1 != p2.get(); -} - -template <class C> -inline bool operator!=(const C* p1, const scoped_ptr<const C>& p2) { - return p1 != p2.get(); -} - -template <class C> -scoped_ptr<C> make_scoped_ptr(C *p) { - // This does nothing but to return a scoped_ptr of the type that the passed - // pointer is of. (This eliminates the need to specify the name of T when - // making a scoped_ptr that is used anonymously/temporarily.) From an - // access control point of view, we construct an unnamed scoped_ptr here - // which we return and thus copy-construct. Hence, we need to have access - // to scoped_ptr::scoped_ptr(scoped_ptr const &). However, it is guaranteed - // that we never actually call the copy constructor, which is a good thing - // as we would call the temporary's object destructor (and thus delete p) - // if we actually did copy some object, here. - return scoped_ptr<C>(p); -} - -// scoped_array<C> is like scoped_ptr<C>, except that the caller must allocate -// with new [] and the destructor deletes objects with delete []. -// -// As with scoped_ptr<C>, a scoped_array<C> either points to an object -// or is NULL. A scoped_array<C> owns the object that it points to. -// scoped_array<T> is thread-compatible, and once you index into it, -// the returned objects have only the threadsafety guarantees of T. -// -// Size: sizeof(scoped_array<C>) == sizeof(C*) -template <class C> -class scoped_array { - public: - // The element type - typedef C element_type; - - // Constructor. Defaults to intializing with NULL. - // There is no way to create an uninitialized scoped_array. - // The input parameter must be allocated with new []. - explicit scoped_array(C* p = NULL) : array_(p) { } - - // Destructor. If there is a C object, delete it. - // We don't need to test ptr_ == NULL because C++ does that for us. - ~scoped_array() { - enum { type_must_be_complete = sizeof(C) }; - delete[] array_; - } - - // Reset. Deletes the current owned object, if any. - // Then takes ownership of a new object, if given. - // this->reset(this->get()) works. - void reset(C* p = NULL) { - if (p != array_) { - enum { type_must_be_complete = sizeof(C) }; - delete[] array_; - array_ = p; - } - } - - // Get one element of the current object. - // Will assert() if there is no current object, or index i is negative. - C& operator[](std::ptrdiff_t i) const { - assert(i >= 0); - assert(array_ != NULL); - return array_[i]; - } - - // Get a pointer to the zeroth element of the current object. - // If there is no current object, return NULL. - C* get() const { - return array_; - } - - // Comparison operators. - // These return whether a scoped_array and a raw pointer refer to - // the same array, not just to two different but equal arrays. - bool operator==(const C* p) const { return array_ == p; } - bool operator!=(const C* p) const { return array_ != p; } - - // Swap two scoped arrays. - void swap(scoped_array& p2) { - C* tmp = array_; - array_ = p2.array_; - p2.array_ = tmp; - } - - // Release an array. - // The return value is the current pointer held by this object. - // If this object holds a NULL pointer, the return value is NULL. - // After this operation, this object will hold a NULL pointer, - // and will not own the object any more. - C* release() { - C* retVal = array_; - array_ = NULL; - return retVal; - } - - private: - C* array_; - - // Forbid comparison of different scoped_array types. - template <class C2> bool operator==(scoped_array<C2> const& p2) const; - template <class C2> bool operator!=(scoped_array<C2> const& p2) const; - - // Disallow evil constructors - scoped_array(const scoped_array&); - void operator=(const scoped_array&); -}; - -// Free functions -template <class C> -inline void swap(scoped_array<C>& p1, scoped_array<C>& p2) { - p1.swap(p2); -} - -template <class C> -inline bool operator==(const C* p1, const scoped_array<C>& p2) { - return p1 == p2.get(); -} - -template <class C> -inline bool operator==(const C* p1, const scoped_array<const C>& p2) { - return p1 == p2.get(); -} - -template <class C> -inline bool operator!=(const C* p1, const scoped_array<C>& p2) { - return p1 != p2.get(); -} - -template <class C> -inline bool operator!=(const C* p1, const scoped_array<const C>& p2) { - return p1 != p2.get(); -} - -// This class wraps the c library function free() in a class that can be -// passed as a template argument to scoped_ptr_malloc below. -class ScopedPtrMallocFree { - public: - inline void operator()(void* x) const { - free(x); - } -}; - -} // namespace internal -} // namespace ceres - -#endif // CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ diff --git a/extern/ceres/include/ceres/internal/variadic_evaluate.h b/extern/ceres/include/ceres/internal/variadic_evaluate.h index b3515b96d18..046832c0bb4 100644 --- a/extern/ceres/include/ceres/internal/variadic_evaluate.h +++ b/extern/ceres/include/ceres/internal/variadic_evaluate.h @@ -28,165 +28,77 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) // mierle@gmail.com (Keir Mierle) +// jodebo_beck@gmx.de (Johannes Beck) #ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ #define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ #include <stddef.h> -#include "ceres/jet.h" -#include "ceres/types.h" -#include "ceres/internal/eigen.h" -#include "ceres/internal/fixed_array.h" -#include "glog/logging.h" +#include <type_traits> +#include <utility> + +#include "ceres/cost_function.h" +#include "ceres/internal/parameter_dims.h" namespace ceres { namespace internal { -// This block of quasi-repeated code calls the user-supplied functor, which may -// take a variable number of arguments. This is accomplished by specializing the -// struct based on the size of the trailing parameters; parameters with 0 size -// are assumed missing. -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8, int N9> -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - input[7], - input[8], - input[9], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, N8, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - input[7], - input[8], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - input[7], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, - int N5, int N6> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, - int N5> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1, int N2, int N3> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1, int N2> -struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1> -struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - output); - } -}; - -template<typename Functor, typename T, int N0> -struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - output); - } -}; - -// Template instantiation for dynamically-sized functors. -template<typename Functor, typename T> -struct VariadicEvaluate<Functor, T, ceres::DYNAMIC, ceres::DYNAMIC, - ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC, - ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC, - ceres::DYNAMIC, ceres::DYNAMIC> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input, output); - } -}; +// For fixed size cost functors +template <typename Functor, typename T, int... Indices> +inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input, + T* output, std::false_type /*is_dynamic*/, + std::integer_sequence<int, Indices...>) { + static_assert(sizeof...(Indices), + "Invalid number of parameter blocks. At least one parameter " + "block must be specified."); + return functor(input[Indices]..., output); +} + +// For dynamic sized cost functors +template <typename Functor, typename T> +inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input, + T* output, std::true_type /*is_dynamic*/, + std::integer_sequence<int>) { + return functor(input, output); +} + +// For ceres cost functors (not ceres::CostFunction) +template <typename ParameterDims, typename Functor, typename T> +inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input, + T* output, const void* /* NOT USED */) { + using ParameterBlockIndices = + std::make_integer_sequence<int, ParameterDims::kNumParameterBlocks>; + using IsDynamic = std::integral_constant<bool, ParameterDims::kIsDynamic>; + return VariadicEvaluateImpl(functor, input, output, IsDynamic(), + ParameterBlockIndices()); +} + +// For ceres::CostFunction +template <typename ParameterDims, typename Functor, typename T> +inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input, + T* output, + const CostFunction* /* NOT USED */) { + return functor.Evaluate(input, output, nullptr); +} + +// Variadic evaluate is a helper function to evaluate ceres cost function or +// functors using an input, output and the parameter dimensions. There are +// several ways different possibilities: +// 1) If the passed functor is a 'ceres::CostFunction' its evaluate method is +// called. +// 2) If the functor is not a 'ceres::CostFunction' and the specified parameter +// dims is dynamic, the functor must have the following signature +// 'bool(T const* const* input, T* output)'. +// 3) If the functor is not a 'ceres::CostFunction' and the specified parameter +// dims is not dynamic, the input is expanded by using the number of parameter +// blocks. The signature of the functor must have the following signature +// 'bool()(const T* i_1, const T* i_2, ... const T* i_n, T* output)'. +template <typename ParameterDims, typename Functor, typename T> +inline bool VariadicEvaluate(const Functor& functor, T const* const* input, + T* output) { + return VariadicEvaluateImpl<ParameterDims>(functor, input, output, &functor); +} } // namespace internal } // namespace ceres diff --git a/extern/ceres/include/ceres/iteration_callback.h b/extern/ceres/include/ceres/iteration_callback.h index db5d0efe53a..0a743ecc26f 100644 --- a/extern/ceres/include/ceres/iteration_callback.h +++ b/extern/ceres/include/ceres/iteration_callback.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -35,42 +35,22 @@ #ifndef CERES_PUBLIC_ITERATION_CALLBACK_H_ #define CERES_PUBLIC_ITERATION_CALLBACK_H_ -#include "ceres/types.h" #include "ceres/internal/disable_warnings.h" +#include "ceres/types.h" namespace ceres { // This struct describes the state of the optimizer after each // iteration of the minimization. struct CERES_EXPORT IterationSummary { - IterationSummary() - : iteration(0), - step_is_valid(false), - step_is_nonmonotonic(false), - step_is_successful(false), - cost(0.0), - cost_change(0.0), - gradient_max_norm(0.0), - gradient_norm(0.0), - step_norm(0.0), - eta(0.0), - step_size(0.0), - line_search_function_evaluations(0), - line_search_gradient_evaluations(0), - line_search_iterations(0), - linear_solver_iterations(0), - iteration_time_in_seconds(0.0), - step_solver_time_in_seconds(0.0), - cumulative_time_in_seconds(0.0) {} - // Current iteration number. - int32 iteration; + int iteration = 0; // 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 always true when iteration = 0. - bool step_is_valid; + bool step_is_valid = false; // Step did not reduce the value of the objective function // sufficiently, but it was accepted because of the relaxed @@ -78,7 +58,7 @@ struct CERES_EXPORT IterationSummary { // algorithm. // // Note: step_is_nonmonotonic is always false when iteration = 0; - bool step_is_nonmonotonic; + bool step_is_nonmonotonic = false; // Whether or not the minimizer accepted this step or not. If the // ordinary trust region algorithm is used, this means that the @@ -90,68 +70,68 @@ struct CERES_EXPORT IterationSummary { // step and the step is declared successful. // // Note: step_is_successful is always true when iteration = 0. - bool step_is_successful; + bool step_is_successful = false; // Value of the objective function. - double cost; + double cost = 0.90; // Change in the value of the objective function in this // iteration. This can be positive or negative. - double cost_change; + double cost_change = 0.0; // Infinity norm of the gradient vector. - double gradient_max_norm; + double gradient_max_norm = 0.0; // 2-norm of the gradient vector. - double gradient_norm; + double gradient_norm = 0.0; // 2-norm of the size of the step computed by the optimization // algorithm. - double step_norm; + double step_norm = 0.0; // For trust region algorithms, the ratio of the actual change in // cost and the change in the cost of the linearized approximation. - double relative_decrease; + double relative_decrease = 0.0; // Size of the trust region at the end of the current iteration. For // the Levenberg-Marquardt algorithm, the regularization parameter // mu = 1.0 / trust_region_radius. - double trust_region_radius; + double trust_region_radius = 0.0; // For the inexact step Levenberg-Marquardt algorithm, this is the // relative accuracy with which the Newton(LM) step is solved. This // number affects only the iterative solvers capable of solving // linear systems inexactly. Factorization-based exact solvers // ignore it. - double eta; + double eta = 0.0; // Step sized computed by the line search algorithm. - double step_size; + double step_size = 0.0; // Number of function value evaluations used by the line search algorithm. - int line_search_function_evaluations; + int line_search_function_evaluations = 0; // Number of function gradient evaluations used by the line search algorithm. - int line_search_gradient_evaluations; + int line_search_gradient_evaluations = 0; // Number of iterations taken by the line search algorithm. - int line_search_iterations; + int line_search_iterations = 0; // Number of iterations taken by the linear solver to solve for the // Newton step. - int linear_solver_iterations; + int linear_solver_iterations = 0; // All times reported below are wall times. // Time (in seconds) spent inside the minimizer loop in the current // iteration. - double iteration_time_in_seconds; + double iteration_time_in_seconds = 0.0; // Time (in seconds) spent inside the trust region step solver. - double step_solver_time_in_seconds; + double step_solver_time_in_seconds = 0.0; // Time (in seconds) since the user called Solve(). - double cumulative_time_in_seconds; + double cumulative_time_in_seconds = 0.0; }; // Interface for specifying callbacks that are executed at the end of diff --git a/extern/ceres/include/ceres/jet.h b/extern/ceres/include/ceres/jet.h index a104707298c..7aafaa01d30 100644 --- a/extern/ceres/include/ceres/jet.h +++ b/extern/ceres/include/ceres/jet.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,7 +31,7 @@ // A simple implementation of N-dimensional dual numbers, for automatically // computing exact derivatives of functions. // -// While a complete treatment of the mechanics of automatic differentation is +// While a complete treatment of the mechanics of automatic differentiation is // beyond the scope of this header (see // http://en.wikipedia.org/wiki/Automatic_differentiation for details), the // basic idea is to extend normal arithmetic with an extra element, "e," often @@ -49,7 +49,7 @@ // f(x) = x^2 , // // evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20. -// Next, augument 10 with an infinitesimal to get: +// Next, argument 10 with an infinitesimal to get: // // f(10 + e) = (10 + e)^2 // = 100 + 2 * 10 * e + e^2 @@ -102,8 +102,9 @@ // } // // // The "2" means there should be 2 dual number components. -// Jet<double, 2> x(0); // Pick the 0th dual number for x. -// Jet<double, 2> y(1); // Pick the 1st dual number for y. +// // It computes the partial derivative at x=10, y=20. +// Jet<double, 2> x(10, 0); // Pick the 0th dual number for x. +// Jet<double, 2> y(20, 1); // Pick the 1st dual number for y. // Jet<double, 2> z = f(x, y); // // LOG(INFO) << "df/dx = " << z.v[0] @@ -124,7 +125,7 @@ // // x = a + \sum_i v[i] t_i // -// A shorthand is to write an element as x = a + u, where u is the pertubation. +// A shorthand is to write an element as x = a + u, where u is the perturbation. // Then, the main point about the arithmetic of jets is that the product of // perturbations is zero: // @@ -163,7 +164,6 @@ #include <string> #include "Eigen/Core" -#include "ceres/fpclassify.h" #include "ceres/internal/port.h" namespace ceres { @@ -171,26 +171,25 @@ namespace ceres { template <typename T, int N> struct Jet { enum { DIMENSION = N }; + typedef T Scalar; // Default-construct "a" because otherwise this can lead to false errors about // uninitialized uses when other classes relying on default constructed T // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that // the C++ standard mandates that e.g. default constructed doubles are // initialized to 0.0; see sections 8.5 of the C++03 standard. - Jet() : a() { - v.setZero(); - } + Jet() : a() { v.setConstant(Scalar()); } // Constructor from scalar: a + 0. explicit Jet(const T& value) { a = value; - v.setZero(); + v.setConstant(Scalar()); } // Constructor from scalar plus variable: a + t_i. Jet(const T& value, int k) { a = value; - v.setZero(); + v.setConstant(Scalar()); v[k] = T(1.0); } @@ -198,58 +197,66 @@ struct Jet { // The use of Eigen::DenseBase allows Eigen expressions // to be passed in without being fully evaluated until // they are assigned to v - template<typename Derived> - EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived> &v) - : a(a), v(v) { - } + template <typename Derived> + EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived>& v) + : a(a), v(v) {} // Compound operators - Jet<T, N>& operator+=(const Jet<T, N> &y) { + Jet<T, N>& operator+=(const Jet<T, N>& y) { *this = *this + y; return *this; } - Jet<T, N>& operator-=(const Jet<T, N> &y) { + Jet<T, N>& operator-=(const Jet<T, N>& y) { *this = *this - y; return *this; } - Jet<T, N>& operator*=(const Jet<T, N> &y) { + Jet<T, N>& operator*=(const Jet<T, N>& y) { *this = *this * y; return *this; } - Jet<T, N>& operator/=(const Jet<T, N> &y) { + Jet<T, N>& operator/=(const Jet<T, N>& y) { *this = *this / y; return *this; } + // Compound with scalar operators. + Jet<T, N>& operator+=(const T& s) { + *this = *this + s; + return *this; + } + + Jet<T, N>& operator-=(const T& s) { + *this = *this - s; + return *this; + } + + Jet<T, N>& operator*=(const T& s) { + *this = *this * s; + return *this; + } + + Jet<T, N>& operator/=(const T& s) { + *this = *this / s; + return *this; + } + // The scalar part. T a; // The infinitesimal part. + Eigen::Matrix<T, N, 1> v; - // 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 + // This struct needs to have an Eigen aligned operator new as it contains + // fixed-size Eigen types. + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // Unary + -template<typename T, int N> inline -Jet<T, N> const& operator+(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> const& operator+(const Jet<T, N>& f) { return f; } @@ -257,72 +264,68 @@ Jet<T, N> const& operator+(const Jet<T, N>& f) { // see if it causes a performance increase. // Unary - -template<typename T, int N> inline -Jet<T, N> operator-(const Jet<T, N>&f) { +template <typename T, int N> +inline Jet<T, N> operator-(const Jet<T, N>& f) { return Jet<T, N>(-f.a, -f.v); } // Binary + -template<typename T, int N> inline -Jet<T, N> operator+(const Jet<T, N>& f, - const Jet<T, N>& g) { +template <typename T, int N> +inline Jet<T, N> operator+(const Jet<T, N>& f, const Jet<T, N>& g) { return Jet<T, N>(f.a + g.a, f.v + g.v); } // Binary + with a scalar: x + s -template<typename T, int N> inline -Jet<T, N> operator+(const Jet<T, N>& f, T s) { +template <typename T, int N> +inline Jet<T, N> operator+(const Jet<T, N>& f, T s) { return Jet<T, N>(f.a + s, f.v); } // Binary + with a scalar: s + x -template<typename T, int N> inline -Jet<T, N> operator+(T s, const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> operator+(T s, const Jet<T, N>& f) { return Jet<T, N>(f.a + s, f.v); } // Binary - -template<typename T, int N> inline -Jet<T, N> operator-(const Jet<T, N>& f, - const Jet<T, N>& g) { +template <typename T, int N> +inline Jet<T, N> operator-(const Jet<T, N>& f, const Jet<T, N>& g) { return Jet<T, N>(f.a - g.a, f.v - g.v); } // Binary - with a scalar: x - s -template<typename T, int N> inline -Jet<T, N> operator-(const Jet<T, N>& f, T s) { +template <typename T, int N> +inline Jet<T, N> operator-(const Jet<T, N>& f, T s) { return Jet<T, N>(f.a - s, f.v); } // Binary - with a scalar: s - x -template<typename T, int N> inline -Jet<T, N> operator-(T s, const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> operator-(T s, const Jet<T, N>& f) { return Jet<T, N>(s - f.a, -f.v); } // Binary * -template<typename T, int N> inline -Jet<T, N> operator*(const Jet<T, N>& f, - const Jet<T, N>& g) { +template <typename T, int N> +inline Jet<T, N> operator*(const Jet<T, N>& f, const Jet<T, N>& g) { return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a); } // Binary * with a scalar: x * s -template<typename T, int N> inline -Jet<T, N> operator*(const Jet<T, N>& f, T s) { +template <typename T, int N> +inline Jet<T, N> operator*(const Jet<T, N>& f, T s) { return Jet<T, N>(f.a * s, f.v * s); } // Binary * with a scalar: s * x -template<typename T, int N> inline -Jet<T, N> operator*(T s, const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> operator*(T s, const Jet<T, N>& f) { return Jet<T, N>(f.a * s, f.v * s); } // Binary / -template<typename T, int N> inline -Jet<T, N> operator/(const Jet<T, N>& f, - const Jet<T, N>& g) { +template <typename T, int N> +inline Jet<T, N> operator/(const Jet<T, N>& f, const Jet<T, N>& g) { // This uses: // // a + u (a + u)(b - v) (a + u)(b - v) @@ -332,43 +335,43 @@ Jet<T, N> operator/(const Jet<T, N>& f, // which holds because v*v = 0. const T g_a_inverse = T(1.0) / g.a; const T f_a_by_g_a = f.a * g_a_inverse; - return Jet<T, N>(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse); + return Jet<T, N>(f_a_by_g_a, (f.v - f_a_by_g_a * g.v) * g_a_inverse); } // Binary / with a scalar: s / x -template<typename T, int N> inline -Jet<T, N> operator/(T s, const Jet<T, N>& g) { +template <typename T, int N> +inline Jet<T, N> operator/(T s, const Jet<T, N>& g) { const T minus_s_g_a_inverse2 = -s / (g.a * g.a); return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2); } // Binary / with a scalar: x / s -template<typename T, int N> inline -Jet<T, N> operator/(const Jet<T, N>& f, T s) { - const T s_inverse = 1.0 / s; +template <typename T, int N> +inline Jet<T, N> operator/(const Jet<T, N>& f, T s) { + const T s_inverse = T(1.0) / s; return Jet<T, N>(f.a * s_inverse, f.v * s_inverse); } // Binary comparison operators for both scalars and jets. -#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ -template<typename T, int N> inline \ -bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \ - return f.a op g.a; \ -} \ -template<typename T, int N> inline \ -bool operator op(const T& s, const Jet<T, N>& g) { \ - return s op g.a; \ -} \ -template<typename T, int N> inline \ -bool operator op(const Jet<T, N>& f, const T& s) { \ - return f.a op s; \ -} -CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT -CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT -CERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT -CERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT -CERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT -CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT +#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ + template <typename T, int N> \ + inline bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \ + return f.a op g.a; \ + } \ + template <typename T, int N> \ + inline bool operator op(const T& s, const Jet<T, N>& g) { \ + return s op g.a; \ + } \ + template <typename T, int N> \ + inline bool operator op(const Jet<T, N>& f, const T& s) { \ + return f.a op s; \ + } +CERES_DEFINE_JET_COMPARISON_OPERATOR(<) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(<=) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(>) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(>=) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(==) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(!=) // NOLINT #undef CERES_DEFINE_JET_COMPARISON_OPERATOR // Pull some functions from namespace std. @@ -376,112 +379,128 @@ CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT // This is necessary because we want to use the same name (e.g. 'sqrt') for // double-valued and Jet-valued functions, but we are not allowed to put // Jet-valued functions inside namespace std. -// -// TODO(keir): Switch to "using". -inline double abs (double x) { return std::abs(x); } -inline double log (double x) { return std::log(x); } -inline double exp (double x) { return std::exp(x); } -inline double sqrt (double x) { return std::sqrt(x); } -inline double cos (double x) { return std::cos(x); } -inline double acos (double x) { return std::acos(x); } -inline double sin (double x) { return std::sin(x); } -inline double asin (double x) { return std::asin(x); } -inline double tan (double x) { return std::tan(x); } -inline double atan (double x) { return std::atan(x); } -inline double sinh (double x) { return std::sinh(x); } -inline double cosh (double x) { return std::cosh(x); } -inline double tanh (double x) { return std::tanh(x); } -inline double 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); } +using std::abs; +using std::acos; +using std::asin; +using std::atan; +using std::atan2; +using std::cbrt; +using std::ceil; +using std::cos; +using std::cosh; +using std::exp; +using std::exp2; +using std::floor; +using std::fmax; +using std::fmin; +using std::hypot; +using std::isfinite; +using std::isinf; +using std::isnan; +using std::isnormal; +using std::log; +using std::log2; +using std::pow; +using std::sin; +using std::sinh; +using std::sqrt; +using std::tan; +using std::tanh; + +// Legacy names from pre-C++11 days. +// clang-format off +inline bool IsFinite(double x) { return std::isfinite(x); } +inline bool IsInfinite(double x) { return std::isinf(x); } +inline bool IsNaN(double x) { return std::isnan(x); } +inline bool IsNormal(double x) { return std::isnormal(x); } +// clang-format on // In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. // abs(x + h) ~= x + h or -(x + h) -template <typename T, int N> inline -Jet<T, N> abs(const Jet<T, N>& f) { - return f.a < T(0.0) ? -f : f; +template <typename T, int N> +inline Jet<T, N> abs(const Jet<T, N>& f) { + return (f.a < T(0.0) ? -f : f); } // log(a + h) ~= log(a) + h / a -template <typename T, int N> inline -Jet<T, N> log(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> log(const Jet<T, N>& f) { const T a_inverse = T(1.0) / f.a; return Jet<T, N>(log(f.a), f.v * a_inverse); } // exp(a + h) ~= exp(a) + exp(a) h -template <typename T, int N> inline -Jet<T, N> exp(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> exp(const Jet<T, N>& f) { const T tmp = exp(f.a); return Jet<T, N>(tmp, tmp * f.v); } // sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) -template <typename T, int N> inline -Jet<T, N> sqrt(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> sqrt(const Jet<T, N>& f) { const T tmp = sqrt(f.a); const T two_a_inverse = T(1.0) / (T(2.0) * tmp); return Jet<T, N>(tmp, f.v * two_a_inverse); } // cos(a + h) ~= cos(a) - sin(a) h -template <typename T, int N> inline -Jet<T, N> cos(const Jet<T, N>& f) { - return Jet<T, N>(cos(f.a), - sin(f.a) * f.v); +template <typename T, int N> +inline Jet<T, N> cos(const Jet<T, N>& f) { + return Jet<T, N>(cos(f.a), -sin(f.a) * f.v); } // acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h -template <typename T, int N> inline -Jet<T, N> acos(const Jet<T, N>& f) { - const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a); +template <typename T, int N> +inline Jet<T, N> acos(const Jet<T, N>& f) { + const T tmp = -T(1.0) / sqrt(T(1.0) - f.a * f.a); return Jet<T, N>(acos(f.a), tmp * f.v); } // sin(a + h) ~= sin(a) + cos(a) h -template <typename T, int N> inline -Jet<T, N> sin(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> sin(const Jet<T, N>& f) { return Jet<T, N>(sin(f.a), cos(f.a) * f.v); } // asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h -template <typename T, int N> inline -Jet<T, N> asin(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> asin(const Jet<T, N>& f) { const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); return Jet<T, N>(asin(f.a), tmp * f.v); } // tan(a + h) ~= tan(a) + (1 + tan(a)^2) h -template <typename T, int N> inline -Jet<T, N> tan(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> tan(const Jet<T, N>& f) { const T tan_a = tan(f.a); const T tmp = T(1.0) + tan_a * tan_a; return Jet<T, N>(tan_a, tmp * f.v); } // atan(a + h) ~= atan(a) + 1 / (1 + a^2) h -template <typename T, int N> inline -Jet<T, N> atan(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> atan(const Jet<T, N>& f) { const T tmp = T(1.0) / (T(1.0) + f.a * f.a); return Jet<T, N>(atan(f.a), tmp * f.v); } // sinh(a + h) ~= sinh(a) + cosh(a) h -template <typename T, int N> inline -Jet<T, N> sinh(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> sinh(const Jet<T, N>& f) { return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v); } // cosh(a + h) ~= cosh(a) + sinh(a) h -template <typename T, int N> inline -Jet<T, N> cosh(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> cosh(const Jet<T, N>& f) { return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v); } // tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h -template <typename T, int N> inline -Jet<T, N> tanh(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> tanh(const Jet<T, N>& f) { const T tanh_a = tanh(f.a); const T tmp = T(1.0) - tanh_a * tanh_a; return Jet<T, N>(tanh_a, tmp * f.v); @@ -491,8 +510,8 @@ Jet<T, N> tanh(const Jet<T, N>& f) { // 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) { +template <typename T, int N> +inline Jet<T, N> floor(const Jet<T, N>& f) { return Jet<T, N>(floor(f.a)); } @@ -500,11 +519,60 @@ Jet<T, N> floor(const Jet<T, N>& f) { // 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) { +template <typename T, int N> +inline Jet<T, N> ceil(const Jet<T, N>& f) { return Jet<T, N>(ceil(f.a)); } +// Some new additions to C++11: + +// cbrt(a + h) ~= cbrt(a) + h / (3 a ^ (2/3)) +template <typename T, int N> +inline Jet<T, N> cbrt(const Jet<T, N>& f) { + const T derivative = T(1.0) / (T(3.0) * cbrt(f.a * f.a)); + return Jet<T, N>(cbrt(f.a), f.v * derivative); +} + +// exp2(x + h) = 2^(x+h) ~= 2^x + h*2^x*log(2) +template <typename T, int N> +inline Jet<T, N> exp2(const Jet<T, N>& f) { + const T tmp = exp2(f.a); + const T derivative = tmp * log(T(2)); + return Jet<T, N>(tmp, f.v * derivative); +} + +// log2(x + h) ~= log2(x) + h / (x * log(2)) +template <typename T, int N> +inline Jet<T, N> log2(const Jet<T, N>& f) { + const T derivative = T(1.0) / (f.a * log(T(2))); + return Jet<T, N>(log2(f.a), f.v * derivative); +} + +// Like sqrt(x^2 + y^2), +// but acts to prevent underflow/overflow for small/large x/y. +// Note that the function is non-smooth at x=y=0, +// so the derivative is undefined there. +template <typename T, int N> +inline Jet<T, N> hypot(const Jet<T, N>& x, const Jet<T, N>& y) { + // d/da sqrt(a) = 0.5 / sqrt(a) + // d/dx x^2 + y^2 = 2x + // So by the chain rule: + // d/dx sqrt(x^2 + y^2) = 0.5 / sqrt(x^2 + y^2) * 2x = x / sqrt(x^2 + y^2) + // d/dy sqrt(x^2 + y^2) = y / sqrt(x^2 + y^2) + const T tmp = hypot(x.a, y.a); + return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v); +} + +template <typename T, int N> +inline Jet<T, N> fmax(const Jet<T, N>& x, const Jet<T, N>& y) { + return x < y ? y : x; +} + +template <typename T, int N> +inline Jet<T, N> fmin(const Jet<T, N>& x, const Jet<T, N>& y) { + return y < x ? y : x; +} + // Bessel functions of the first kind with integer order equal to 0, 1, n. // // Microsoft has deprecated the j[0,1,n]() POSIX Bessel functions in favour of @@ -512,21 +580,21 @@ Jet<T, N> ceil(const Jet<T, N>& f) { // 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) +#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) return _j0(x); #else return j0(x); #endif } inline double BesselJ1(double x) { -#if defined(_MSC_VER) && defined(_j1) +#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) return _j1(x); #else return j1(x); #endif } inline double BesselJn(int n, double x) { -#if defined(_MSC_VER) && defined(_jn) +#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) return _jn(n, x); #else return jn(n, x); @@ -541,32 +609,32 @@ inline double BesselJn(int n, double x) { // See formula http://dlmf.nist.gov/10.6#E3 // j0(a + h) ~= j0(a) - j1(a) h -template <typename T, int N> inline -Jet<T, N> BesselJ0(const Jet<T, N>& f) { - return Jet<T, N>(BesselJ0(f.a), - -BesselJ1(f.a) * f.v); +template <typename T, int N> +inline Jet<T, N> BesselJ0(const Jet<T, N>& f) { + return Jet<T, N>(BesselJ0(f.a), -BesselJ1(f.a) * f.v); } // See formula http://dlmf.nist.gov/10.6#E1 // j1(a + h) ~= j1(a) + 0.5 ( j0(a) - j2(a) ) h -template <typename T, int N> inline -Jet<T, N> BesselJ1(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> BesselJ1(const Jet<T, N>& f) { return Jet<T, N>(BesselJ1(f.a), T(0.5) * (BesselJ0(f.a) - BesselJn(2, f.a)) * f.v); } // See formula http://dlmf.nist.gov/10.6#E1 // j_n(a + h) ~= j_n(a) + 0.5 ( j_{n-1}(a) - j_{n+1}(a) ) h -template <typename T, int N> inline -Jet<T, N> BesselJn(int n, const Jet<T, N>& f) { - return Jet<T, N>(BesselJn(n, f.a), - T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v); +template <typename T, int N> +inline Jet<T, N> BesselJn(int n, const Jet<T, N>& f) { + return Jet<T, N>( + BesselJn(n, f.a), + T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v); } // Jet Classification. It is not clear what the appropriate semantics are for -// these classifications. This picks that IsFinite and isnormal are "all" -// operations, i.e. all elements of the jet must be finite for the jet itself -// to be finite (or normal). For IsNaN and IsInfinite, the answer is less +// these classifications. This picks that std::isfinite and std::isnormal are +// "all" operations, i.e. all elements of the jet must be finite for the jet +// itself to be finite (or normal). For IsNaN and IsInfinite, the answer is less // clear. This takes a "any" approach for IsNaN and IsInfinite such that if any // part of a jet is nan or inf, then the entire jet is nan or inf. This leads // to strange situations like a jet can be both IsInfinite and IsNaN, but in @@ -574,81 +642,88 @@ Jet<T, N> BesselJn(int n, const Jet<T, N>& f) { // derivatives are sane. // The jet is finite if all parts of the jet are finite. -template <typename T, int N> inline -bool IsFinite(const Jet<T, N>& f) { - if (!IsFinite(f.a)) { - return false; - } +template <typename T, int N> +inline bool isfinite(const Jet<T, N>& f) { + // Branchless implementation. This is more efficient for the false-case and + // works with the codegen system. + auto result = isfinite(f.a); for (int i = 0; i < N; ++i) { - if (!IsFinite(f.v[i])) { - return false; - } + result = result & isfinite(f.v[i]); } - return true; + return result; } -// The jet is infinite if any part of the jet is infinite. -template <typename T, int N> inline -bool IsInfinite(const Jet<T, N>& f) { - if (IsInfinite(f.a)) { - return true; - } - for (int i = 0; i < N; i++) { - if (IsInfinite(f.v[i])) { - return true; - } +// The jet is infinite if any part of the Jet is infinite. +template <typename T, int N> +inline bool isinf(const Jet<T, N>& f) { + auto result = isinf(f.a); + for (int i = 0; i < N; ++i) { + result = result | isinf(f.v[i]); } - return false; + return result; } // The jet is NaN if any part of the jet is NaN. -template <typename T, int N> inline -bool IsNaN(const Jet<T, N>& f) { - if (IsNaN(f.a)) { - return true; - } +template <typename T, int N> +inline bool isnan(const Jet<T, N>& f) { + auto result = isnan(f.a); for (int i = 0; i < N; ++i) { - if (IsNaN(f.v[i])) { - return true; - } + result = result | isnan(f.v[i]); } - return false; + return result; } // The jet is normal if all parts of the jet are normal. -template <typename T, int N> inline -bool IsNormal(const Jet<T, N>& f) { - if (!IsNormal(f.a)) { - return false; - } +template <typename T, int N> +inline bool isnormal(const Jet<T, N>& f) { + auto result = isnormal(f.a); for (int i = 0; i < N; ++i) { - if (!IsNormal(f.v[i])) { - return false; - } + result = result & isnormal(f.v[i]); } - return true; + return result; +} + +// Legacy functions from the pre-C++11 days. +template <typename T, int N> +inline bool IsFinite(const Jet<T, N>& f) { + return isfinite(f); +} + +template <typename T, int N> +inline bool IsNaN(const Jet<T, N>& f) { + return isnan(f); +} + +template <typename T, int N> +inline bool IsNormal(const Jet<T, N>& f) { + return isnormal(f); +} + +// The jet is infinite if any part of the jet is infinite. +template <typename T, int N> +inline bool IsInfinite(const Jet<T, N>& f) { + return isinf(f); } // atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) // // In words: the rate of change of theta is 1/r times the rate of // change of (x, y) in the positive angular direction. -template <typename T, int N> inline -Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { // Note order of arguments: // // f = a + da // g = b + db T const tmp = T(1.0) / (f.a * f.a + g.a * g.a); - return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v)); + return Jet<T, N>(atan2(g.a, f.a), tmp * (-g.a * f.v + f.a * g.v)); } - // pow -- base is a differentiable function, exponent is a constant. // (a+da)^p ~= a^p + p*a^(p-1) da -template <typename T, int N> inline -Jet<T, N> pow(const Jet<T, N>& f, double g) { +template <typename T, int N> +inline Jet<T, N> pow(const Jet<T, N>& f, double g) { T const tmp = g * pow(f.a, g - T(1.0)); return Jet<T, N>(pow(f.a, g), tmp * f.v); } @@ -664,26 +739,30 @@ Jet<T, N> pow(const Jet<T, N>& f, double g) { // 3. For f < 0 and integer g we have: (f)^(g + dg) ~= f^g but if dg // != 0, the derivatives are not defined and we return NaN. -template <typename T, int N> inline -Jet<T, N> pow(double f, const Jet<T, N>& g) { - if (f == 0 && g.a > 0) { +template <typename T, int N> +inline Jet<T, N> pow(T f, const Jet<T, N>& g) { + Jet<T, N> result; + + if (f == T(0) && g.a > T(0)) { // Handle case 2. - return Jet<T, N>(T(0.0)); - } - if (f < 0 && g.a == floor(g.a)) { - // Handle case 3. - Jet<T, N> ret(pow(f, g.a)); - for (int i = 0; i < N; i++) { - if (g.v[i] != T(0.0)) { - // Return a NaN when g.v != 0. - ret.v[i] = std::numeric_limits<T>::quiet_NaN(); + result = Jet<T, N>(T(0.0)); + } else { + if (f < 0 && g.a == floor(g.a)) { // Handle case 3. + result = Jet<T, N>(pow(f, g.a)); + for (int i = 0; i < N; i++) { + if (g.v[i] != T(0.0)) { + // Return a NaN when g.v != 0. + result.v[i] = std::numeric_limits<T>::quiet_NaN(); + } } + } else { + // Handle case 1. + T const tmp = pow(f, g.a); + result = Jet<T, N>(tmp, log(f) * tmp * g.v); } - return ret; } - // Handle case 1. - T const tmp = pow(f, g.a); - return Jet<T, N>(tmp, log(f) * tmp * g.v); + + return result; } // pow -- both base and exponent are differentiable functions. This has a @@ -722,73 +801,48 @@ Jet<T, N> pow(double f, const Jet<T, N>& g) { // // 9. For f < 0, g noninteger: The value and derivatives of f^g are not finite. -template <typename T, int N> inline -Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { - if (f.a == 0 && g.a >= 1) { +template <typename T, int N> +inline Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { + Jet<T, N> result; + + if (f.a == T(0) && g.a >= T(1)) { // Handle cases 2 and 3. - if (g.a > 1) { - return Jet<T, N>(T(0.0)); + if (g.a > T(1)) { + result = Jet<T, N>(T(0.0)); + } else { + result = f; } - return f; - } - if (f.a < 0 && g.a == floor(g.a)) { - // Handle cases 7 and 8. - T const tmp = g.a * pow(f.a, g.a - T(1.0)); - Jet<T, N> ret(pow(f.a, g.a), tmp * f.v); - for (int i = 0; i < N; i++) { - if (g.v[i] != T(0.0)) { - // Return a NaN when g.v != 0. - ret.v[i] = std::numeric_limits<T>::quiet_NaN(); + + } else { + if (f.a < T(0) && g.a == floor(g.a)) { + // Handle cases 7 and 8. + T const tmp = g.a * pow(f.a, g.a - T(1.0)); + result = Jet<T, N>(pow(f.a, g.a), tmp * f.v); + for (int i = 0; i < N; i++) { + if (g.v[i] != T(0.0)) { + // Return a NaN when g.v != 0. + result.v[i] = T(std::numeric_limits<double>::quiet_NaN()); + } } + } else { + // Handle the remaining cases. For cases 4,5,6,9 we allow the log() + // function to generate -HUGE_VAL or NaN, since those cases result in a + // nonfinite derivative. + T const tmp1 = pow(f.a, g.a); + T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); + T const tmp3 = tmp1 * log(f.a); + result = Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v); } - return ret; } - // Handle the remaining cases. For cases 4,5,6,9 we allow the log() function - // to generate -HUGE_VAL or NaN, since those cases result in a nonfinite - // derivative. - T const tmp1 = pow(f.a, g.a); - T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); - T const tmp3 = tmp1 * log(f.a); - return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v); -} - -// Define the helper functions Eigen needs to embed Jet types. -// -// NOTE(keir): machine_epsilon() and precision() are missing, because they don't -// work with nested template types (e.g. where the scalar is itself templated). -// Among other things, this means that decompositions of Jet's does not work, -// for example -// -// Matrix<Jet<T, N> ... > A, x, b; -// ... -// A.solve(b, &x) -// -// does not work and will fail with a strange compiler error. -// -// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we -// switch to 3.0, also add the rest of the specialization functionality. -template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x; } // NOLINT -template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x; } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_imag(const Jet<T, N>& ) { return Jet<T, N>(0.0); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_abs (const Jet<T, N>& x) { return fabs(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_abs2(const Jet<T, N>& x) { return x * x; } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_sqrt(const Jet<T, N>& x) { return sqrt(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) { return exp(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_tan (const Jet<T, N>& x) { return tan(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_atan(const Jet<T, N>& x) { return atan(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_sinh(const Jet<T, N>& x) { return sinh(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_cosh(const Jet<T, N>& x) { return cosh(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_tanh(const Jet<T, N>& x) { return tanh(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_pow (const Jet<T, N>& x, Jet<T, N> y) { return pow(x, y); } // NOLINT + + return result; +} // Note: This has to be in the ceres namespace for argument dependent lookup to // function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with // strange compile errors. template <typename T, int N> -inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) { +inline std::ostream& operator<<(std::ostream& s, const Jet<T, N>& z) { s << "[" << z.a << " ; "; for (int i = 0; i < N; ++i) { s << z.v[i]; @@ -799,15 +853,78 @@ inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) { s << "]"; return s; } - } // namespace ceres +namespace std { +template <typename T, int N> +struct numeric_limits<ceres::Jet<T, N>> { + static constexpr bool is_specialized = true; + static constexpr bool is_signed = std::numeric_limits<T>::is_signed; + static constexpr bool is_integer = std::numeric_limits<T>::is_integer; + static constexpr bool is_exact = std::numeric_limits<T>::is_exact; + static constexpr bool has_infinity = std::numeric_limits<T>::has_infinity; + static constexpr bool has_quiet_NaN = std::numeric_limits<T>::has_quiet_NaN; + static constexpr bool has_signaling_NaN = + std::numeric_limits<T>::has_signaling_NaN; + static constexpr bool is_iec559 = std::numeric_limits<T>::is_iec559; + static constexpr bool is_bounded = std::numeric_limits<T>::is_bounded; + static constexpr bool is_modulo = std::numeric_limits<T>::is_modulo; + + static constexpr std::float_denorm_style has_denorm = + std::numeric_limits<T>::has_denorm; + static constexpr std::float_round_style round_style = + std::numeric_limits<T>::round_style; + + static constexpr int digits = std::numeric_limits<T>::digits; + static constexpr int digits10 = std::numeric_limits<T>::digits10; + static constexpr int max_digits10 = std::numeric_limits<T>::max_digits10; + static constexpr int radix = std::numeric_limits<T>::radix; + static constexpr int min_exponent = std::numeric_limits<T>::min_exponent; + static constexpr int min_exponent10 = std::numeric_limits<T>::max_exponent10; + static constexpr int max_exponent = std::numeric_limits<T>::max_exponent; + static constexpr int max_exponent10 = std::numeric_limits<T>::max_exponent10; + static constexpr bool traps = std::numeric_limits<T>::traps; + static constexpr bool tinyness_before = + std::numeric_limits<T>::tinyness_before; + + static constexpr ceres::Jet<T, N> min() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::min()); + } + static constexpr ceres::Jet<T, N> lowest() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::lowest()); + } + static constexpr ceres::Jet<T, N> epsilon() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::epsilon()); + } + static constexpr ceres::Jet<T, N> round_error() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::round_error()); + } + static constexpr ceres::Jet<T, N> infinity() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::infinity()); + } + static constexpr ceres::Jet<T, N> quiet_NaN() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::quiet_NaN()); + } + static constexpr ceres::Jet<T, N> signaling_NaN() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::signaling_NaN()); + } + static constexpr ceres::Jet<T, N> denorm_min() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::denorm_min()); + } + + static constexpr ceres::Jet<T, N> max() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::max()); + } +}; + +} // namespace std + namespace Eigen { // Creating a specialization of NumTraits enables placing Jet objects inside // Eigen arrays, getting all the goodness of Eigen combined with autodiff. -template<typename T, int N> -struct NumTraits<ceres::Jet<T, N> > { +template <typename T, int N> +struct NumTraits<ceres::Jet<T, N>> { typedef ceres::Jet<T, N> Real; typedef ceres::Jet<T, N> NonInteger; typedef ceres::Jet<T, N> Nested; @@ -821,6 +938,8 @@ struct NumTraits<ceres::Jet<T, N> > { return Real(std::numeric_limits<T>::epsilon()); } + static inline int digits10() { return NumTraits<T>::digits10(); } + enum { IsComplex = 0, IsInteger = 0, @@ -833,7 +952,7 @@ struct NumTraits<ceres::Jet<T, N> > { RequireInitialization = 1 }; - template<bool Vectorized> + template <bool Vectorized> struct Div { enum { #if defined(EIGEN_VECTORIZE_AVX) @@ -847,6 +966,24 @@ struct NumTraits<ceres::Jet<T, N> > { Cost = 3 }; }; + + static inline Real highest() { return Real(std::numeric_limits<T>::max()); } + static inline Real lowest() { return Real(-std::numeric_limits<T>::max()); } +}; + +// Specifying the return type of binary operations between Jets and scalar types +// allows you to perform matrix/array operations with Eigen matrices and arrays +// such as addition, subtraction, multiplication, and division where one Eigen +// matrix/array is of type Jet and the other is a scalar type. This improves +// performance by using the optimized scalar-to-Jet binary operations but +// is only available on Eigen versions >= 3.3 +template <typename BinaryOp, typename T, int N> +struct ScalarBinaryOpTraits<ceres::Jet<T, N>, T, BinaryOp> { + typedef ceres::Jet<T, N> ReturnType; +}; +template <typename BinaryOp, typename T, int N> +struct ScalarBinaryOpTraits<T, ceres::Jet<T, N>, BinaryOp> { + typedef ceres::Jet<T, N> ReturnType; }; } // namespace Eigen diff --git a/extern/ceres/include/ceres/local_parameterization.h b/extern/ceres/include/ceres/local_parameterization.h index 379fc684921..1576e829e73 100644 --- a/extern/ceres/include/ceres/local_parameterization.h +++ b/extern/ceres/include/ceres/local_parameterization.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -32,10 +32,12 @@ #ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ #define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ +#include <array> +#include <memory> #include <vector> -#include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" + #include "ceres/internal/disable_warnings.h" +#include "ceres/internal/port.h" namespace ceres { @@ -61,7 +63,7 @@ namespace ceres { // optimize over two dimensional vector delta in the tangent space at // that point and then "move" to the point x + delta, where the move // operation involves projecting back onto the sphere. Doing so -// removes a redundent dimension from the optimization, making it +// removes a redundant dimension from the optimization, making it // numerically more robust and efficient. // // More generally we can define a function @@ -154,17 +156,16 @@ class CERES_EXPORT IdentityParameterization : public LocalParameterization { public: explicit IdentityParameterization(int size); virtual ~IdentityParameterization() {} - virtual bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const; - virtual bool ComputeJacobian(const double* x, - double* jacobian) const; - virtual bool MultiplyByJacobian(const double* x, - const int num_cols, - const double* global_matrix, - double* local_matrix) const; - virtual int GlobalSize() const { return size_; } - virtual int LocalSize() const { return size_; } + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + bool MultiplyByJacobian(const double* x, + const int num_cols, + const double* global_matrix, + double* local_matrix) const override; + int GlobalSize() const override { return size_; } + int LocalSize() const override { return size_; } private: const int size_; @@ -176,19 +177,18 @@ class CERES_EXPORT SubsetParameterization : public LocalParameterization { explicit SubsetParameterization(int size, const std::vector<int>& constant_parameters); virtual ~SubsetParameterization() {} - virtual bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const; - virtual bool ComputeJacobian(const double* x, - double* jacobian) const; - virtual bool MultiplyByJacobian(const double* x, - const int num_cols, - const double* global_matrix, - double* local_matrix) const; - virtual int GlobalSize() const { + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + bool MultiplyByJacobian(const double* x, + const int num_cols, + const double* global_matrix, + double* local_matrix) const override; + int GlobalSize() const override { return static_cast<int>(constancy_mask_.size()); } - virtual int LocalSize() const { return local_size_; } + int LocalSize() const override { return local_size_; } private: const int local_size_; @@ -202,13 +202,12 @@ class CERES_EXPORT SubsetParameterization : public LocalParameterization { class CERES_EXPORT QuaternionParameterization : public LocalParameterization { public: virtual ~QuaternionParameterization() {} - virtual bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const; - virtual bool ComputeJacobian(const double* x, - double* jacobian) const; - virtual int GlobalSize() const { return 4; } - virtual int LocalSize() const { return 3; } + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override { return 4; } + int LocalSize() const override { return 3; } }; // Implements the quaternion local parameterization for Eigen's representation @@ -222,16 +221,16 @@ class CERES_EXPORT QuaternionParameterization : public LocalParameterization { // // Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x // with * being the quaternion multiplication operator. -class EigenQuaternionParameterization : public ceres::LocalParameterization { +class CERES_EXPORT 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; } + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override { return 4; } + int LocalSize() const override { return 3; } }; // This provides a parameterization for homogeneous vectors which are commonly @@ -247,32 +246,55 @@ class EigenQuaternionParameterization : public ceres::LocalParameterization { // remain on the sphere. We assume that the last element of x is the scalar // component. The size of the homogeneous vector is required to be greater than // 1. -class CERES_EXPORT HomogeneousVectorParameterization : - public LocalParameterization { +class CERES_EXPORT HomogeneousVectorParameterization + : public LocalParameterization { public: explicit HomogeneousVectorParameterization(int size); virtual ~HomogeneousVectorParameterization() {} - virtual bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const; - virtual bool ComputeJacobian(const double* x, - double* jacobian) const; - virtual int GlobalSize() const { return size_; } - virtual int LocalSize() const { return size_ - 1; } + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override { return size_; } + int LocalSize() const override { return size_ - 1; } private: const int size_; }; +// This provides a parameterization for lines, where the line is +// over-parameterized by an origin point and a direction vector. So the +// parameter vector size needs to be two times the ambient space dimension, +// where the first half is interpreted as the origin point and the second half +// as the direction. +// +// The plus operator for the line direction is the same as for the +// HomogeneousVectorParameterization. The update of the origin point is +// perpendicular to the line direction before the update. +// +// This local parameterization is a special case of the affine Grassmannian +// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold)) +// for the case Graff_1(R^n). +template <int AmbientSpaceDimension> +class LineParameterization : public LocalParameterization { + public: + static_assert(AmbientSpaceDimension >= 2, + "The ambient space must be at least 2"); + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override { return 2 * AmbientSpaceDimension; } + int LocalSize() const override { return 2 * (AmbientSpaceDimension - 1); } +}; + // Construct a local parameterization by taking the Cartesian product // of a number of other local parameterizations. This is useful, when // a parameter block is the cartesian product of two or more // manifolds. For example the parameters of a camera consist of a // rotation and a translation, i.e., SO(3) x R^3. // -// Currently this class supports taking the cartesian product of up to -// four local parameterizations. -// // Example usage: // // ProductParameterization product_param(new QuaterionionParameterization(), @@ -282,35 +304,49 @@ class CERES_EXPORT HomogeneousVectorParameterization : // rotation is represented using a quaternion. class CERES_EXPORT ProductParameterization : public LocalParameterization { public: + ProductParameterization(const ProductParameterization&) = delete; + ProductParameterization& operator=(const ProductParameterization&) = delete; // - // NOTE: All the constructors take ownership of the input local + // NOTE: The constructor takes ownership of the input local // parameterizations. // - ProductParameterization(LocalParameterization* local_param1, - LocalParameterization* local_param2); + template <typename... LocalParams> + ProductParameterization(LocalParams*... local_params) + : local_params_(sizeof...(LocalParams)), + local_size_{0}, + global_size_{0}, + buffer_size_{0} { + constexpr int kNumLocalParams = sizeof...(LocalParams); + static_assert(kNumLocalParams >= 2, + "At least two local parameterizations must be specified."); - ProductParameterization(LocalParameterization* local_param1, - LocalParameterization* local_param2, - LocalParameterization* local_param3); + using LocalParameterizationPtr = std::unique_ptr<LocalParameterization>; - ProductParameterization(LocalParameterization* local_param1, - LocalParameterization* local_param2, - LocalParameterization* local_param3, - LocalParameterization* local_param4); + // Wrap all raw pointers into std::unique_ptr for exception safety. + std::array<LocalParameterizationPtr, kNumLocalParams> local_params_array{ + LocalParameterizationPtr(local_params)...}; - virtual ~ProductParameterization(); - virtual bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const; - virtual bool ComputeJacobian(const double* x, - double* jacobian) const; - virtual int GlobalSize() const { return global_size_; } - virtual int LocalSize() const { return local_size_; } + // Initialize internal state. + for (int i = 0; i < kNumLocalParams; ++i) { + LocalParameterizationPtr& param = local_params_[i]; + param = std::move(local_params_array[i]); - private: - void Init(); + buffer_size_ = + std::max(buffer_size_, param->LocalSize() * param->GlobalSize()); + global_size_ += param->GlobalSize(); + local_size_ += param->LocalSize(); + } + } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override { return global_size_; } + int LocalSize() const override { return local_size_; } - std::vector<LocalParameterization*> local_params_; + private: + std::vector<std::unique_ptr<LocalParameterization>> local_params_; int local_size_; int global_size_; int buffer_size_; @@ -319,5 +355,7 @@ class CERES_EXPORT ProductParameterization : public LocalParameterization { } // namespace ceres #include "ceres/internal/reenable_warnings.h" +#include "ceres/internal/line_parameterization.h" #endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ + diff --git a/extern/ceres/include/ceres/loss_function.h b/extern/ceres/include/ceres/loss_function.h index 0512c135143..7aabf7dfce1 100644 --- a/extern/ceres/include/ceres/loss_function.h +++ b/extern/ceres/include/ceres/loss_function.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -57,7 +57,7 @@ // anything special (i.e. if we used a basic quadratic loss), the // residual for the erroneous measurement will result in extreme error // due to the quadratic nature of squared loss. This results in the -// entire solution getting pulled away from the optimimum to reduce +// entire solution getting pulled away from the optimum to reduce // the large error that would otherwise be attributed to the wrong // measurement. // @@ -75,11 +75,11 @@ #ifndef CERES_PUBLIC_LOSS_FUNCTION_H_ #define CERES_PUBLIC_LOSS_FUNCTION_H_ -#include "glog/logging.h" -#include "ceres/internal/macros.h" -#include "ceres/internal/scoped_ptr.h" -#include "ceres/types.h" +#include <memory> + #include "ceres/internal/disable_warnings.h" +#include "ceres/types.h" +#include "glog/logging.h" namespace ceres { @@ -119,7 +119,6 @@ class CERES_EXPORT LossFunction { // Note: in the region of interest (i.e. s < 3) we have: // TrivialLoss >= HuberLoss >= SoftLOneLoss >= CauchyLoss - // This corresponds to no robustification. // // rho(s) = s @@ -131,7 +130,7 @@ class CERES_EXPORT LossFunction { // thing. class CERES_EXPORT TrivialLoss : public LossFunction { public: - virtual void Evaluate(double, double*) const; + void Evaluate(double, double*) const override; }; // Scaling @@ -174,8 +173,8 @@ class CERES_EXPORT TrivialLoss : public LossFunction { // http://en.wikipedia.org/wiki/Huber_Loss_Function class CERES_EXPORT HuberLoss : public LossFunction { public: - explicit HuberLoss(double a) : a_(a), b_(a * a) { } - virtual void Evaluate(double, double*) const; + explicit HuberLoss(double a) : a_(a), b_(a * a) {} + void Evaluate(double, double*) const override; private: const double a_; @@ -187,11 +186,11 @@ class CERES_EXPORT HuberLoss : public LossFunction { // // rho(s) = 2 (sqrt(1 + s) - 1). // -// At s = 0: rho = [0, 1, -1/2]. +// At s = 0: rho = [0, 1, -1 / (2 * a^2)]. class CERES_EXPORT SoftLOneLoss : public LossFunction { public: - explicit SoftLOneLoss(double a) : b_(a * a), c_(1 / b_) { } - virtual void Evaluate(double, double*) const; + explicit SoftLOneLoss(double a) : b_(a * a), c_(1 / b_) {} + void Evaluate(double, double*) const override; private: // b = a^2. @@ -204,11 +203,11 @@ class CERES_EXPORT SoftLOneLoss : public LossFunction { // // rho(s) = log(1 + s). // -// At s = 0: rho = [0, 1, -1]. +// At s = 0: rho = [0, 1, -1 / a^2]. class CERES_EXPORT CauchyLoss : public LossFunction { public: - explicit CauchyLoss(double a) : b_(a * a), c_(1 / b_) { } - virtual void Evaluate(double, double*) const; + explicit CauchyLoss(double a) : b_(a * a), c_(1 / b_) {} + void Evaluate(double, double*) const override; private: // b = a^2. @@ -228,8 +227,8 @@ class CERES_EXPORT CauchyLoss : public LossFunction { // At s = 0: rho = [0, 1, 0]. class CERES_EXPORT ArctanLoss : public LossFunction { public: - explicit ArctanLoss(double a) : a_(a), b_(1 / (a * a)) { } - virtual void Evaluate(double, double*) const; + explicit ArctanLoss(double a) : a_(a), b_(1 / (a * a)) {} + void Evaluate(double, double*) const override; private: const double a_; @@ -268,7 +267,7 @@ class CERES_EXPORT ArctanLoss : public LossFunction { class CERES_EXPORT TolerantLoss : public LossFunction { public: explicit TolerantLoss(double a, double b); - virtual void Evaluate(double, double*) const; + void Evaluate(double, double*) const override; private: const double a_, b_, c_; @@ -277,16 +276,17 @@ class CERES_EXPORT TolerantLoss : public LossFunction { // This is the Tukey biweight loss function which aggressively // attempts to suppress large errors. // -// The term is computed as: +// The term is computed as follows where the equations are scaled by a +// factor of 2 because the cost function is given by 1/2 rho(s): // -// rho(s) = a^2 / 6 * (1 - (1 - s / a^2)^3 ) for s <= a^2, -// rho(s) = a^2 / 6 for s > a^2. +// rho(s) = a^2 / 3 * (1 - (1 - s / a^2)^3 ) for s <= a^2, +// rho(s) = a^2 / 3 for s > a^2. // -// At s = 0: rho = [0, 0.5, -1 / a^2] +// At s = 0: rho = [0, 1, -2 / a^2] class CERES_EXPORT TukeyLoss : public ceres::LossFunction { public: - explicit TukeyLoss(double a) : a_squared_(a * a) { } - virtual void Evaluate(double, double*) const; + explicit TukeyLoss(double a) : a_squared_(a * a) {} + void Evaluate(double, double*) const override; private: const double a_squared_; @@ -297,13 +297,15 @@ class CERES_EXPORT TukeyLoss : public ceres::LossFunction { // The loss functions must not be NULL. class CERES_EXPORT ComposedLoss : public LossFunction { public: - explicit ComposedLoss(const LossFunction* f, Ownership ownership_f, - const LossFunction* g, Ownership ownership_g); + explicit ComposedLoss(const LossFunction* f, + Ownership ownership_f, + const LossFunction* g, + Ownership ownership_g); virtual ~ComposedLoss(); - virtual void Evaluate(double, double*) const; + void Evaluate(double, double*) const override; private: - internal::scoped_ptr<const LossFunction> f_, g_; + std::unique_ptr<const LossFunction> f_, g_; const Ownership ownership_f_, ownership_g_; }; @@ -329,21 +331,22 @@ class CERES_EXPORT ScaledLoss : public LossFunction { // Constructs a ScaledLoss wrapping another loss function. Takes // ownership of the wrapped loss function or not depending on the // ownership parameter. - ScaledLoss(const LossFunction* rho, double a, Ownership ownership) : - rho_(rho), a_(a), ownership_(ownership) { } + ScaledLoss(const LossFunction* rho, double a, Ownership ownership) + : rho_(rho), a_(a), ownership_(ownership) {} + ScaledLoss(const ScaledLoss&) = delete; + void operator=(const ScaledLoss&) = delete; virtual ~ScaledLoss() { if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { rho_.release(); } } - virtual void Evaluate(double, double*) const; + void Evaluate(double, double*) const override; private: - internal::scoped_ptr<const LossFunction> rho_; + std::unique_ptr<const LossFunction> rho_; const double a_; const Ownership ownership_; - CERES_DISALLOW_COPY_AND_ASSIGN(ScaledLoss); }; // Sometimes after the optimization problem has been constructed, we @@ -387,8 +390,10 @@ class CERES_EXPORT ScaledLoss : public LossFunction { class CERES_EXPORT LossFunctionWrapper : public LossFunction { public: LossFunctionWrapper(LossFunction* rho, Ownership ownership) - : rho_(rho), ownership_(ownership) { - } + : rho_(rho), ownership_(ownership) {} + + LossFunctionWrapper(const LossFunctionWrapper&) = delete; + void operator=(const LossFunctionWrapper&) = delete; virtual ~LossFunctionWrapper() { if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { @@ -396,13 +401,12 @@ class CERES_EXPORT LossFunctionWrapper : public LossFunction { } } - virtual void Evaluate(double sq_norm, double out[3]) const { + void Evaluate(double sq_norm, double out[3]) const override { if (rho_.get() == NULL) { out[0] = sq_norm; out[1] = 1.0; out[2] = 0.0; - } - else { + } else { rho_->Evaluate(sq_norm, out); } } @@ -416,9 +420,8 @@ class CERES_EXPORT LossFunctionWrapper : public LossFunction { } private: - internal::scoped_ptr<const LossFunction> rho_; + std::unique_ptr<const LossFunction> rho_; Ownership ownership_; - CERES_DISALLOW_COPY_AND_ASSIGN(LossFunctionWrapper); }; } // namespace ceres diff --git a/extern/ceres/include/ceres/normal_prior.h b/extern/ceres/include/ceres/normal_prior.h index cd98b4c846b..14ab379f4af 100644 --- a/extern/ceres/include/ceres/normal_prior.h +++ b/extern/ceres/include/ceres/normal_prior.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -35,8 +35,8 @@ #define CERES_PUBLIC_NORMAL_PRIOR_H_ #include "ceres/cost_function.h" -#include "ceres/internal/eigen.h" #include "ceres/internal/disable_warnings.h" +#include "ceres/internal/eigen.h" namespace ceres { @@ -57,15 +57,15 @@ namespace ceres { // which would be the case if the covariance matrix S is rank // deficient. -class CERES_EXPORT NormalPrior: public CostFunction { +class CERES_EXPORT NormalPrior : public CostFunction { public: // Check that the number of rows in the vector b are the same as the // number of columns in the matrix A, crash otherwise. NormalPrior(const Matrix& A, const Vector& b); + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override; - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const; private: Matrix A_; Vector b_; diff --git a/extern/ceres/include/ceres/numeric_diff_cost_function.h b/extern/ceres/include/ceres/numeric_diff_cost_function.h index 5dfaeab6241..c69f262f572 100644 --- a/extern/ceres/include/ceres/numeric_diff_cost_function.h +++ b/extern/ceres/include/ceres/numeric_diff_cost_function.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -52,16 +52,16 @@ // The actual cost added to the total problem is e^2, or (k - x'k)^2; however, // the squaring is implicitly done by the optimization framework. // -// To write an numerically-differentiable cost function for the above model, first -// define the object +// To write an numerically-differentiable cost function for the above model, +// first define the object // // class MyScalarCostFunctor { -// MyScalarCostFunctor(double k): k_(k) {} +// explicit MyScalarCostFunctor(double k): k_(k) {} // // bool operator()(const double* const x, // const double* const y, // double* residuals) const { -// residuals[0] = k_ - x[0] * y[0] + x[1] * y[1]; +// residuals[0] = k_ - x[0] * y[0] - x[1] * y[1]; // return true; // } // @@ -98,6 +98,8 @@ // NumericDiffCostFunction also supports cost functions with a // runtime-determined number of residuals. For example: // +// clang-format off +// // CostFunction* cost_function // = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>( // new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^ @@ -109,10 +111,8 @@ // Indicate dynamic number of residuals --------------------+ | | // Dimension of x ------------------------------------------------+ | // Dimension of y ---------------------------------------------------+ +// clang-format on // -// The framework can currently accommodate cost functions of up to 10 -// independent variables, and there is no limit on the dimensionality -// of each of them. // // The central difference method is considerably more accurate at the cost of // twice as many function evaluations than forward difference. Consider using @@ -161,10 +161,13 @@ #ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ #define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ +#include <array> +#include <memory> + #include "Eigen/Dense" #include "ceres/cost_function.h" #include "ceres/internal/numeric_diff.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/internal/parameter_dims.h" #include "ceres/numeric_diff_options.h" #include "ceres/sized_cost_function.h" #include "ceres/types.h" @@ -175,34 +178,17 @@ namespace ceres { template <typename CostFunctor, NumericDiffMethodType method = CENTRAL, int kNumResiduals = 0, // Number of residuals, or ceres::DYNAMIC - int N0 = 0, // Number of parameters in block 0. - int N1 = 0, // Number of parameters in block 1. - int N2 = 0, // Number of parameters in block 2. - int N3 = 0, // Number of parameters in block 3. - int N4 = 0, // Number of parameters in block 4. - int N5 = 0, // Number of parameters in block 5. - int N6 = 0, // Number of parameters in block 6. - int N7 = 0, // Number of parameters in block 7. - int N8 = 0, // Number of parameters in block 8. - int N9 = 0> // Number of parameters in block 9. -class NumericDiffCostFunction - : public SizedCostFunction<kNumResiduals, - N0, N1, N2, N3, N4, - N5, N6, N7, N8, N9> { + int... Ns> // Parameters dimensions for each block. +class NumericDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> { public: NumericDiffCostFunction( CostFunctor* functor, Ownership ownership = TAKE_OWNERSHIP, int num_residuals = kNumResiduals, const NumericDiffOptions& options = NumericDiffOptions()) - : functor_(functor), - ownership_(ownership), - options_(options) { + : functor_(functor), ownership_(ownership), options_(options) { if (kNumResiduals == DYNAMIC) { - SizedCostFunction<kNumResiduals, - N0, N1, N2, N3, N4, - N5, N6, N7, N8, N9> - ::set_num_residuals(num_residuals); + SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals); } } @@ -212,24 +198,21 @@ class NumericDiffCostFunction } } - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override { using internal::FixedArray; using internal::NumericDiff; - const int kNumParameters = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; - const int kNumParameterBlocks = - (N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) + - (N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0); + using ParameterDims = + typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims; + + constexpr int kNumParameters = ParameterDims::kNumParameters; + constexpr int kNumParameterBlocks = ParameterDims::kNumParameterBlocks; // Get the function value (residuals) at the the point to evaluate. - if (!internal::EvaluateImpl<CostFunctor, - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( - functor_.get(), - parameters, - residuals, - functor_.get())) { + if (!internal::VariadicEvaluate<ParameterDims>( + *functor_, parameters, residuals)) { return false; } @@ -239,77 +222,29 @@ class NumericDiffCostFunction // Create a copy of the parameters which will get mutated. FixedArray<double> parameters_copy(kNumParameters); - FixedArray<double*> parameters_reference_copy(kNumParameterBlocks); - - parameters_reference_copy[0] = parameters_copy.get(); - if (N1) parameters_reference_copy[1] = parameters_reference_copy[0] + N0; - if (N2) parameters_reference_copy[2] = parameters_reference_copy[1] + N1; - if (N3) parameters_reference_copy[3] = parameters_reference_copy[2] + N2; - if (N4) parameters_reference_copy[4] = parameters_reference_copy[3] + N3; - if (N5) parameters_reference_copy[5] = parameters_reference_copy[4] + N4; - if (N6) parameters_reference_copy[6] = parameters_reference_copy[5] + N5; - if (N7) parameters_reference_copy[7] = parameters_reference_copy[6] + N6; - if (N8) parameters_reference_copy[8] = parameters_reference_copy[7] + N7; - if (N9) parameters_reference_copy[9] = parameters_reference_copy[8] + N8; + std::array<double*, kNumParameterBlocks> parameters_reference_copy = + ParameterDims::GetUnpackedParameters(parameters_copy.data()); -#define CERES_COPY_PARAMETER_BLOCK(block) \ - if (N ## block) memcpy(parameters_reference_copy[block], \ - parameters[block], \ - sizeof(double) * N ## block); // NOLINT - - CERES_COPY_PARAMETER_BLOCK(0); - CERES_COPY_PARAMETER_BLOCK(1); - CERES_COPY_PARAMETER_BLOCK(2); - CERES_COPY_PARAMETER_BLOCK(3); - CERES_COPY_PARAMETER_BLOCK(4); - CERES_COPY_PARAMETER_BLOCK(5); - CERES_COPY_PARAMETER_BLOCK(6); - CERES_COPY_PARAMETER_BLOCK(7); - CERES_COPY_PARAMETER_BLOCK(8); - CERES_COPY_PARAMETER_BLOCK(9); - -#undef CERES_COPY_PARAMETER_BLOCK - -#define CERES_EVALUATE_JACOBIAN_FOR_BLOCK(block) \ - if (N ## block && jacobians[block] != NULL) { \ - if (!NumericDiff<CostFunctor, \ - method, \ - kNumResiduals, \ - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9, \ - block, \ - N ## block >::EvaluateJacobianForParameterBlock( \ - functor_.get(), \ - residuals, \ - options_, \ - SizedCostFunction<kNumResiduals, \ - N0, N1, N2, N3, N4, \ - N5, N6, N7, N8, N9>::num_residuals(), \ - block, \ - N ## block, \ - parameters_reference_copy.get(), \ - jacobians[block])) { \ - return false; \ - } \ + for (int block = 0; block < kNumParameterBlocks; ++block) { + memcpy(parameters_reference_copy[block], + parameters[block], + sizeof(double) * ParameterDims::GetDim(block)); } - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(0); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(1); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(2); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(3); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(4); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(5); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(6); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(7); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(8); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(9); - -#undef CERES_EVALUATE_JACOBIAN_FOR_BLOCK + internal::EvaluateJacobianForParameterBlocks<ParameterDims>:: + template Apply<method, kNumResiduals>( + functor_.get(), + residuals, + options_, + SizedCostFunction<kNumResiduals, Ns...>::num_residuals(), + parameters_reference_copy.data(), + jacobians); return true; } private: - internal::scoped_ptr<CostFunctor> functor_; + std::unique_ptr<CostFunctor> functor_; Ownership ownership_; NumericDiffOptions options_; }; diff --git a/extern/ceres/include/ceres/numeric_diff_options.h b/extern/ceres/include/ceres/numeric_diff_options.h index 119c8a86596..64919ed5ab1 100644 --- a/extern/ceres/include/ceres/numeric_diff_options.h +++ b/extern/ceres/include/ceres/numeric_diff_options.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -32,23 +32,17 @@ #ifndef CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_ #define CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_ +#include "ceres/internal/port.h" + namespace ceres { // Options pertaining to numeric differentiation (e.g., convergence criteria, // step sizes). struct CERES_EXPORT NumericDiffOptions { - NumericDiffOptions() { - relative_step_size = 1e-6; - ridders_relative_initial_step_size = 1e-2; - max_num_ridders_extrapolations = 10; - ridders_epsilon = 1e-12; - ridders_step_shrink_factor = 2.0; - } - // Numeric differentiation step size (multiplied by parameter block's // order of magnitude). If parameters are close to zero, the step size // is set to sqrt(machine_epsilon). - double relative_step_size; + double relative_step_size = 1e-6; // Initial step size for Ridders adaptive numeric differentiation (multiplied // by parameter block's order of magnitude). @@ -59,19 +53,19 @@ struct CERES_EXPORT NumericDiffOptions { // Note: For Ridders' method to converge, the step size should be initialized // to a value that is large enough to produce a significant change in the // function. As the derivative is estimated, the step size decreases. - double ridders_relative_initial_step_size; + double ridders_relative_initial_step_size = 1e-2; // Maximal number of adaptive extrapolations (sampling) in Ridders' method. - int max_num_ridders_extrapolations; + int max_num_ridders_extrapolations = 10; // Convergence criterion on extrapolation error for Ridders adaptive // differentiation. The available error estimation methods are defined in // NumericDiffErrorType and set in the "ridders_error_method" field. - double ridders_epsilon; + double ridders_epsilon = 1e-12; // The factor in which to shrink the step size with each extrapolation in // Ridders' method. - double ridders_step_shrink_factor; + double ridders_step_shrink_factor = 2.0; }; } // namespace ceres diff --git a/extern/ceres/include/ceres/ordered_groups.h b/extern/ceres/include/ceres/ordered_groups.h index aa1bd3a7da1..954663c97e6 100644 --- a/extern/ceres/include/ceres/ordered_groups.h +++ b/extern/ceres/include/ceres/ordered_groups.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -33,7 +33,9 @@ #include <map> #include <set> +#include <unordered_map> #include <vector> + #include "ceres/internal/port.h" #include "glog/logging.h" @@ -63,8 +65,7 @@ class OrderedGroups { return false; } - typename std::map<T, int>::const_iterator it = - element_to_group_.find(element); + auto it = element_to_group_.find(element); if (it != element_to_group_.end()) { if (it->second == group) { // Element is already in the right group, nothing to do. @@ -126,17 +127,14 @@ class OrderedGroups { return; } - typename std::map<int, std::set<T> >::reverse_iterator it = - group_to_elements_.rbegin(); - std::map<int, std::set<T> > new_group_to_elements; + auto it = group_to_elements_.rbegin(); + std::map<int, std::set<T>> new_group_to_elements; new_group_to_elements[it->first] = it->second; int new_group_id = it->first + 1; for (++it; it != group_to_elements_.rend(); ++it) { - for (typename std::set<T>::const_iterator element_it = it->second.begin(); - element_it != it->second.end(); - ++element_it) { - element_to_group_[*element_it] = new_group_id; + for (const auto& element : it->second) { + element_to_group_[element] = new_group_id; } new_group_to_elements[new_group_id] = it->second; new_group_id++; @@ -148,8 +146,7 @@ class OrderedGroups { // Return the group id for the element. If the element is not a // member of any group, return -1. int GroupId(const T element) const { - typename std::map<T, int>::const_iterator it = - element_to_group_.find(element); + auto it = element_to_group_.find(element); if (it == element_to_group_.end()) { return -1; } @@ -157,27 +154,21 @@ class OrderedGroups { } bool IsMember(const T element) const { - typename std::map<T, int>::const_iterator it = - element_to_group_.find(element); + auto it = element_to_group_.find(element); return (it != element_to_group_.end()); } // This function always succeeds, i.e., implicitly there exists a // group for every integer. int GroupSize(const int group) const { - typename std::map<int, std::set<T> >::const_iterator it = - group_to_elements_.find(group); - return (it == group_to_elements_.end()) ? 0 : it->second.size(); + auto it = group_to_elements_.find(group); + return (it == group_to_elements_.end()) ? 0 : it->second.size(); } - int NumElements() const { - return element_to_group_.size(); - } + int NumElements() const { return element_to_group_.size(); } // Number of groups with one or more elements. - int NumGroups() const { - return group_to_elements_.size(); - } + int NumGroups() const { return group_to_elements_.size(); } // The first group with one or more elements. Calling this when // there are no groups with non-zero elements will result in a @@ -187,17 +178,15 @@ class OrderedGroups { return group_to_elements_.begin()->first; } - const std::map<int, std::set<T> >& group_to_elements() const { + const std::map<int, std::set<T>>& group_to_elements() const { return group_to_elements_; } - const std::map<T, int>& element_to_group() const { - return element_to_group_; - } + const std::map<T, int>& element_to_group() const { return element_to_group_; } private: - std::map<int, std::set<T> > group_to_elements_; - std::map<T, int> element_to_group_; + std::map<int, std::set<T>> group_to_elements_; + std::unordered_map<T, int> element_to_group_; }; // Typedef for the most commonly used version of OrderedGroups. diff --git a/extern/ceres/include/ceres/problem.h b/extern/ceres/include/ceres/problem.h index 27ed4ef15da..88f99663f65 100644 --- a/extern/ceres/include/ceres/problem.h +++ b/extern/ceres/include/ceres/problem.h @@ -34,22 +34,23 @@ #ifndef CERES_PUBLIC_PROBLEM_H_ #define CERES_PUBLIC_PROBLEM_H_ +#include <array> #include <cstddef> #include <map> +#include <memory> #include <set> #include <vector> -#include "glog/logging.h" -#include "ceres/internal/macros.h" +#include "ceres/context.h" +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" -#include "ceres/internal/disable_warnings.h" - +#include "glog/logging.h" namespace ceres { class CostFunction; +class EvaluationCallback; class LossFunction; class LocalParameterization; class Solver; @@ -114,20 +115,13 @@ typedef internal::ResidualBlock* ResidualBlockId; // // Problem problem; // -// problem.AddResidualBlock(new MyUnaryCostFunction(...), x1); -// problem.AddResidualBlock(new MyBinaryCostFunction(...), x2, x3); +// problem.AddResidualBlock(new MyUnaryCostFunction(...), nullptr, x1); +// problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x3); // // Please see cost_function.h for details of the CostFunction object. class CERES_EXPORT Problem { public: struct CERES_EXPORT Options { - Options() - : cost_function_ownership(TAKE_OWNERSHIP), - loss_function_ownership(TAKE_OWNERSHIP), - local_parameterization_ownership(TAKE_OWNERSHIP), - enable_fast_removal(false), - disable_all_safety_checks(false) {} - // These flags control whether the Problem object owns the cost // functions, loss functions, and parameterizations passed into // the Problem. If set to TAKE_OWNERSHIP, then the problem object @@ -135,25 +129,25 @@ class CERES_EXPORT Problem { // destruction. The destructor is careful to delete the pointers // only once, since sharing cost/loss/parameterizations is // allowed. - Ownership cost_function_ownership; - Ownership loss_function_ownership; - Ownership local_parameterization_ownership; + Ownership cost_function_ownership = TAKE_OWNERSHIP; + Ownership loss_function_ownership = TAKE_OWNERSHIP; + Ownership local_parameterization_ownership = TAKE_OWNERSHIP; // If true, trades memory for faster RemoveResidualBlock() and // RemoveParameterBlock() operations. // // By default, RemoveParameterBlock() and RemoveResidualBlock() take time // proportional to the size of the entire problem. If you only ever remove - // parameters or residuals from the problem occassionally, this might be + // parameters or residuals from the problem occasionally, this might be // acceptable. However, if you have memory to spare, enable this option to // make RemoveParameterBlock() take time proportional to the number of // residual blocks that depend on it, and RemoveResidualBlock() take (on // average) constant time. // - // The increase in memory usage is twofold: an additonal hash set per + // The increase in memory usage is twofold: an additional hash set per // parameter block containing all the residuals that depend on the parameter // block; and a hash set in the problem containing all residuals. - bool enable_fast_removal; + bool enable_fast_removal = false; // By default, Ceres performs a variety of safety checks when constructing // the problem. There is a small but measurable performance penalty to @@ -164,22 +158,51 @@ class CERES_EXPORT Problem { // // WARNING: Do not set this to true, unless you are absolutely sure of what // you are doing. - bool disable_all_safety_checks; + bool disable_all_safety_checks = false; + + // A Ceres global context to use for solving this problem. This may help to + // reduce computation time as Ceres can reuse expensive objects to create. + // The context object can be nullptr, in which case Ceres may create one. + // + // Ceres does NOT take ownership of the pointer. + Context* context = nullptr; + + // Using this callback interface, Ceres can notify you when it is + // about to evaluate the residuals or jacobians. With the + // callback, you can share computation between residual blocks by + // doing the shared computation in + // EvaluationCallback::PrepareForEvaluation() before Ceres calls + // CostFunction::Evaluate(). It also enables caching results + // between a pure residual evaluation and a residual & jacobian + // evaluation. + // + // Problem DOES NOT take ownership of the callback. + // + // NOTE: Evaluation callbacks are incompatible with inner + // iterations. So calling Solve with + // Solver::Options::use_inner_iterations = true on a Problem with + // a non-null evaluation callback is an error. + EvaluationCallback* evaluation_callback = nullptr; }; // The default constructor is equivalent to the // invocation Problem(Problem::Options()). Problem(); explicit Problem(const Options& options); + Problem(Problem&&); + Problem& operator=(Problem&&); + + Problem(const Problem&) = delete; + Problem& operator=(const Problem&) = delete; ~Problem(); // Add a residual block to the overall cost function. The cost - // function carries with it information about the sizes of the + // function carries with its information about the sizes of the // parameter blocks it expects. The function checks that these match // the sizes of the parameter blocks listed in parameter_blocks. The // program aborts if a mismatch is detected. loss_function can be - // NULL, in which case the cost of the term is just the squared norm + // nullptr, in which case the cost of the term is just the squared norm // of the residuals. // // The user has the option of explicitly adding the parameter blocks @@ -208,59 +231,35 @@ class CERES_EXPORT Problem { // // Problem problem; // - // problem.AddResidualBlock(new MyUnaryCostFunction(...), NULL, x1); - // problem.AddResidualBlock(new MyBinaryCostFunction(...), NULL, x2, x1); + // problem.AddResidualBlock(new MyUnaryCostFunction(...), nullptr, x1); + // problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x1); // + // Add a residual block by listing the parameter block pointers + // directly instead of wapping them in a container. + template <typename... Ts> + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, + Ts*... xs) { + const std::array<double*, sizeof...(Ts) + 1> parameter_blocks{{x0, xs...}}; + return AddResidualBlock(cost_function, + loss_function, + parameter_blocks.data(), + static_cast<int>(parameter_blocks.size())); + } + + // Add a residual block by providing a vector of parameter blocks. ResidualBlockId AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, const std::vector<double*>& parameter_blocks); - // Convenience methods for adding residuals with a small number of - // parameters. This is the common case. Instead of specifying the - // parameter block arguments as a vector, list them as pointers. - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1); + // Add a residual block by providing a pointer to the parameter block array + // and the number of parameter blocks. ResidualBlockId AddResidualBlock(CostFunction* cost_function, LossFunction* loss_function, - double* x0, double* x1, double* x2); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4, double* x5); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4, double* x5, - double* x6); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4, double* x5, - double* x6, double* x7); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4, double* x5, - double* x6, double* x7, double* x8); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4, double* x5, - double* x6, double* x7, double* x8, - double* x9); + double* const* const parameter_blocks, + int num_parameter_blocks); // Add a parameter block with appropriate size to the problem. // Repeated calls with the same arguments are ignored. Repeated @@ -290,7 +289,7 @@ class CERES_EXPORT Problem { // ordering, rendering the jacobian or residuals returned from the solver // uninterpretable. If you depend on the evaluated jacobian, do not use // remove! This may change in a future release. - void RemoveParameterBlock(double* values); + void RemoveParameterBlock(const double* values); // Remove a residual block from the problem. Any parameters that the residual // block depends on are not removed. The cost and loss functions for the @@ -304,32 +303,43 @@ class CERES_EXPORT Problem { void RemoveResidualBlock(ResidualBlockId residual_block); // Hold the indicated parameter block constant during optimization. - void SetParameterBlockConstant(double* values); + void SetParameterBlockConstant(const double* values); // 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; + // Returns true if a parameter block is set constant, and false + // otherwise. A parameter block may be set constant in two ways: + // either by calling SetParameterBlockConstant or by associating a + // LocalParameterization with a zero dimensional tangent space with + // it. + bool IsParameterBlockConstant(const 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 // parameters; the destructor is careful to delete local - // parameterizations only once. The local parameterization can only - // be set once per parameter, and cannot be changed once set. + // parameterizations only once. Calling SetParameterization with + // nullptr will clear any previously set parameterization. void SetParameterization(double* values, LocalParameterization* local_parameterization); // Get the local parameterization object associated with this // parameter block. If there is no parameterization object - // associated then NULL is returned. - const LocalParameterization* GetParameterization(double* values) const; + // associated then nullptr is returned. + const LocalParameterization* GetParameterization(const double* values) const; - // Set the lower/upper bound for the parameter with position "index". + // Set the lower/upper bound for the parameter at position "index". void SetParameterLowerBound(double* values, int index, double lower_bound); void SetParameterUpperBound(double* values, int index, double upper_bound); + // Get the lower/upper bound for the parameter at position + // "index". If the parameter is not bounded by the user, then its + // lower bound is -std::numeric_limits<double>::max() and upper + // bound is std::numeric_limits<double>::max(). + double GetParameterLowerBound(const double* values, int index) const; + double GetParameterUpperBound(const double* values, int index) const; + // Number of parameter blocks in the problem. Always equals // parameter_blocks().size() and parameter_block_sizes().size(). int NumParameterBlocks() const; @@ -376,7 +386,7 @@ class CERES_EXPORT Problem { const CostFunction* GetCostFunctionForResidualBlock( const ResidualBlockId residual_block) const; - // Get the LossFunction for the given residual block. Returns NULL + // Get the LossFunction for the given residual block. Returns nullptr // if no loss function is associated with this residual block. const LossFunction* GetLossFunctionForResidualBlock( const ResidualBlockId residual_block) const; @@ -393,11 +403,6 @@ class CERES_EXPORT Problem { // Options struct to control Problem::Evaluate. struct EvaluateOptions { - EvaluateOptions() - : apply_loss_function(true), - num_threads(1) { - } - // The set of parameter blocks for which evaluation should be // performed. This vector determines the order that parameter // blocks occur in the gradient vector and in the columns of the @@ -430,12 +435,12 @@ class CERES_EXPORT Problem { // function. This is of use for example if the user wishes to // analyse the solution quality by studying the distribution of // residuals before and after the solve. - bool apply_loss_function; + bool apply_loss_function = true; - int num_threads; + int num_threads = 1; }; - // Evaluate Problem. Any of the output pointers can be NULL. Which + // Evaluate Problem. Any of the output pointers can be nullptr. Which // residual blocks and parameter blocks are used is controlled by // the EvaluateOptions struct above. // @@ -445,16 +450,16 @@ class CERES_EXPORT Problem { // // Problem problem; // double x = 1; - // problem.AddResidualBlock(new MyCostFunction, NULL, &x); + // problem.AddResidualBlock(new MyCostFunction, nullptr, &x); // // double cost = 0.0; - // problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL); + // problem.Evaluate(Problem::EvaluateOptions(), &cost, nullptr, nullptr, nullptr); // // The cost is evaluated at x = 1. If you wish to evaluate the // problem at x = 2, then // - // x = 2; - // problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL); + // x = 2; + // problem.Evaluate(Problem::EvaluateOptions(), &cost, nullptr, nullptr, nullptr); // // is the way to do so. // @@ -468,17 +473,63 @@ class CERES_EXPORT Problem { // 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. + // + // Note 4: If an EvaluationCallback is associated with the problem, + // then its PrepareForEvaluation method will be called everytime + // this method is called with new_point = true. bool Evaluate(const EvaluateOptions& options, double* cost, std::vector<double>* residuals, std::vector<double>* gradient, CRSMatrix* jacobian); + // Evaluates the residual block, storing the scalar cost in *cost, + // the residual components in *residuals, and the jacobians between + // the parameters and residuals in jacobians[i], in row-major order. + // + // If residuals is nullptr, the residuals are not computed. + // + // If jacobians is nullptr, no Jacobians are computed. If + // jacobians[i] is nullptr, then the Jacobian for that parameter + // block is not computed. + // + // It is not okay to request the Jacobian w.r.t a parameter block + // that is constant. + // + // The return value indicates the success or failure. Even if the + // function returns false, the caller should expect the output + // memory locations to have been modified. + // + // The returned cost and jacobians have had robustification and + // local parameterizations applied already; for example, the + // jacobian for a 4-dimensional quaternion parameter using the + // "QuaternionParameterization" is num_residuals by 3 instead of + // num_residuals by 4. + // + // apply_loss_function as the name implies allows the user to switch + // the application of the loss function on and off. + // + // WARNING: If an EvaluationCallback is associated with the problem + // then it is the user's responsibility to call it before calling + // this method. + // + // This is because, if the user calls this method multiple times, we + // cannot tell if the underlying parameter blocks have changed + // between calls or not. So if EvaluateResidualBlock was responsible + // for calling the EvaluationCallback, it will have to do it + // everytime it is called. Which makes the common case where the + // parameter blocks do not change, inefficient. So we leave it to + // the user to call the EvaluationCallback as needed. + bool EvaluateResidualBlock(ResidualBlockId residual_block_id, + bool apply_loss_function, + double* cost, + double* residuals, + double** jacobians) const; + private: friend class Solver; friend class Covariance; - internal::scoped_ptr<internal::ProblemImpl> problem_impl_; - CERES_DISALLOW_COPY_AND_ASSIGN(Problem); + std::unique_ptr<internal::ProblemImpl> impl_; }; } // namespace ceres diff --git a/extern/ceres/include/ceres/rotation.h b/extern/ceres/include/ceres/rotation.h index b6a06f772c4..7d5c8ef1fb2 100644 --- a/extern/ceres/include/ceres/rotation.h +++ b/extern/ceres/include/ceres/rotation.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -49,6 +49,8 @@ #include <cmath> #include <limits> +#include "glog/logging.h" + namespace ceres { // Trivial wrapper to index linear arrays as matrices, given a fixed @@ -82,47 +84,44 @@ MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer); // and quaternion is a 4-tuple that will contain the resulting quaternion. // The implementation may be used with auto-differentiation up to the first // derivative, higher derivatives may have unexpected results near the origin. -template<typename T> +template <typename T> void AngleAxisToQuaternion(const T* angle_axis, T* quaternion); // Convert a quaternion to the equivalent combined axis-angle representation. // The value quaternion must be a unit quaternion - it is not normalized first, // and angle_axis will be filled with a value whose norm is the angle of // rotation in radians, and whose direction is the axis of rotation. -// The implemention may be used with auto-differentiation up to the first +// The implementation may be used with auto-differentiation up to the first // derivative, higher derivatives may have unexpected results near the origin. -template<typename T> +template <typename T> void QuaternionToAngleAxis(const T* quaternion, T* angle_axis); // Conversions between 3x3 rotation matrix (in column major order) and -// quaternion rotation representations. Templated for use with +// quaternion rotation representations. Templated for use with // autodifferentiation. template <typename T> void RotationMatrixToQuaternion(const T* R, T* quaternion); template <typename T, int row_stride, int col_stride> void RotationMatrixToQuaternion( - const MatrixAdapter<const T, row_stride, col_stride>& R, - T* quaternion); + const MatrixAdapter<const T, row_stride, col_stride>& R, T* quaternion); // Conversions between 3x3 rotation matrix (in column major order) and -// axis-angle rotation representations. Templated for use with +// axis-angle rotation representations. Templated for use with // autodifferentiation. template <typename T> void RotationMatrixToAngleAxis(const T* R, T* angle_axis); template <typename T, int row_stride, int col_stride> void RotationMatrixToAngleAxis( - const MatrixAdapter<const T, row_stride, col_stride>& R, - T* angle_axis); + const MatrixAdapter<const T, row_stride, col_stride>& R, T* angle_axis); template <typename T> void AngleAxisToRotationMatrix(const T* angle_axis, T* R); template <typename T, int row_stride, int col_stride> void AngleAxisToRotationMatrix( - const T* angle_axis, - const MatrixAdapter<T, row_stride, col_stride>& R); + const T* angle_axis, const MatrixAdapter<T, row_stride, col_stride>& R); // Conversions between 3x3 rotation matrix (in row major order) and // Euler angle (in degrees) rotation representations. @@ -135,8 +134,7 @@ void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); template <typename T, int row_stride, int col_stride> void EulerAnglesToRotationMatrix( - const T* euler, - const MatrixAdapter<T, row_stride, col_stride>& R); + const T* euler, const MatrixAdapter<T, row_stride, col_stride>& R); // Convert a 4-vector to a 3x3 scaled rotation matrix. // @@ -157,25 +155,23 @@ void EulerAnglesToRotationMatrix( // such that det(Q) = 1 and Q*Q' = I // // WARNING: The rotation matrix is ROW MAJOR -template <typename T> inline -void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); +template <typename T> +inline void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); -template <typename T, int row_stride, int col_stride> inline -void QuaternionToScaledRotation( - const T q[4], - const MatrixAdapter<T, row_stride, col_stride>& R); +template <typename T, int row_stride, int col_stride> +inline void QuaternionToScaledRotation( + const T q[4], const MatrixAdapter<T, row_stride, col_stride>& R); // Same as above except that the rotation matrix is normalized by the // Frobenius norm, so that R * R' = I (and det(R) = 1). // // WARNING: The rotation matrix is ROW MAJOR -template <typename T> inline -void QuaternionToRotation(const T q[4], T R[3 * 3]); +template <typename T> +inline void QuaternionToRotation(const T q[4], T R[3 * 3]); -template <typename T, int row_stride, int col_stride> inline -void QuaternionToRotation( - const T q[4], - const MatrixAdapter<T, row_stride, col_stride>& R); +template <typename T, int row_stride, int col_stride> +inline void QuaternionToRotation( + const T q[4], const MatrixAdapter<T, row_stride, col_stride>& R); // Rotates a point pt by a quaternion q: // @@ -185,37 +181,54 @@ void QuaternionToRotation( // write the transform as (something)*pt + pt, as is clear from the // formula below. If you pass in a quaternion with |q|^2 = 2 then you // WILL NOT get back 2 times the result you get for a unit quaternion. -template <typename T> inline -void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); +// +// Inplace rotation is not supported. pt and result must point to different +// memory locations, otherwise the result will be undefined. +template <typename T> +inline void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); // With this function you do not need to assume that q has unit norm. // It does assume that the norm is non-zero. -template <typename T> inline -void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); +// +// Inplace rotation is not supported. pt and result must point to different +// memory locations, otherwise the result will be undefined. +template <typename T> +inline void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); // zw = z * w, where * is the Quaternion product between 4 vectors. -template<typename T> inline -void QuaternionProduct(const T z[4], const T w[4], T zw[4]); +// +// Inplace quaternion product is not supported. The resulting quaternion zw must +// not share the memory with the input quaternion z and w, otherwise the result +// will be undefined. +template <typename T> +inline void QuaternionProduct(const T z[4], const T w[4], T zw[4]); // xy = x cross y; -template<typename T> inline -void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); +// +// Inplace cross product is not supported. The resulting vector x_cross_y must +// not share the memory with the input vectors x and y, otherwise the result +// will be undefined. +template <typename T> +inline void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); -template<typename T> inline -T DotProduct(const T x[3], const T y[3]); +template <typename T> +inline T DotProduct(const T x[3], const T y[3]); // y = R(angle_axis) * x; -template<typename T> inline -void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); +// +// Inplace rotation is not supported. pt and result must point to different +// memory locations, otherwise the result will be undefined. +template <typename T> +inline void AngleAxisRotatePoint(const T angle_axis[3], + const T pt[3], + T result[3]); // --- IMPLEMENTATION -template<typename T, int row_stride, int col_stride> +template <typename T, int row_stride, int col_stride> struct MatrixAdapter { T* pointer_; - explicit MatrixAdapter(T* pointer) - : pointer_(pointer) - {} + explicit MatrixAdapter(T* pointer) : pointer_(pointer) {} T& operator()(int r, int c) const { return pointer_[r * row_stride + c * col_stride]; @@ -232,7 +245,7 @@ MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) { return MatrixAdapter<T, 3, 1>(pointer); } -template<typename T> +template <typename T> inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { const T& a0 = angle_axis[0]; const T& a1 = angle_axis[1]; @@ -261,7 +274,7 @@ inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { } } -template<typename T> +template <typename T> inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { const T& q1 = quaternion[1]; const T& q2 = quaternion[2]; @@ -288,9 +301,8 @@ inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { // = atan(-sin(theta), -cos(theta)) // const T two_theta = - T(2.0) * ((cos_theta < 0.0) - ? atan2(-sin_theta, -cos_theta) - : atan2(sin_theta, cos_theta)); + T(2.0) * ((cos_theta < T(0.0)) ? atan2(-sin_theta, -cos_theta) + : atan2(sin_theta, cos_theta)); const T k = two_theta / sin_theta; angle_axis[0] = q1 * k; angle_axis[1] = q2 * k; @@ -316,8 +328,7 @@ void RotationMatrixToQuaternion(const T* R, T* angle_axis) { // Ken Shoemake, 1987 SIGGRAPH course notes template <typename T, int row_stride, int col_stride> void RotationMatrixToQuaternion( - const MatrixAdapter<const T, row_stride, col_stride>& R, - T* quaternion) { + const MatrixAdapter<const T, row_stride, col_stride>& R, T* quaternion) { const T trace = R(0, 0) + R(1, 1) + R(2, 2); if (trace >= 0.0) { T t = sqrt(trace + T(1.0)); @@ -359,8 +370,7 @@ inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) { template <typename T, int row_stride, int col_stride> void RotationMatrixToAngleAxis( - const MatrixAdapter<const T, row_stride, col_stride>& R, - T* angle_axis) { + const MatrixAdapter<const T, row_stride, col_stride>& R, T* angle_axis) { T quaternion[4]; RotationMatrixToQuaternion(R, quaternion); QuaternionToAngleAxis(quaternion, angle_axis); @@ -374,8 +384,7 @@ inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) { template <typename T, int row_stride, int col_stride> void AngleAxisToRotationMatrix( - const T* angle_axis, - const MatrixAdapter<T, row_stride, col_stride>& R) { + const T* angle_axis, const MatrixAdapter<T, row_stride, col_stride>& R) { static const T kOne = T(1.0); const T theta2 = DotProduct(angle_axis, angle_axis); if (theta2 > T(std::numeric_limits<double>::epsilon())) { @@ -390,6 +399,7 @@ void AngleAxisToRotationMatrix( const T costheta = cos(theta); const T sintheta = sin(theta); + // clang-format off R(0, 0) = costheta + wx*wx*(kOne - costheta); R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta); R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta); @@ -399,15 +409,16 @@ void AngleAxisToRotationMatrix( R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta); R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta); R(2, 2) = costheta + wz*wz*(kOne - costheta); + // clang-format on } else { // Near zero, we switch to using the first order Taylor expansion. - R(0, 0) = kOne; - R(1, 0) = angle_axis[2]; + R(0, 0) = kOne; + R(1, 0) = angle_axis[2]; R(2, 0) = -angle_axis[1]; R(0, 1) = -angle_axis[2]; - R(1, 1) = kOne; - R(2, 1) = angle_axis[0]; - R(0, 2) = angle_axis[1]; + R(1, 1) = kOne; + R(2, 1) = angle_axis[0]; + R(0, 2) = angle_axis[1]; R(1, 2) = -angle_axis[0]; R(2, 2) = kOne; } @@ -422,8 +433,7 @@ inline void EulerAnglesToRotationMatrix(const T* euler, template <typename T, int row_stride, int col_stride> void EulerAnglesToRotationMatrix( - const T* euler, - const MatrixAdapter<T, row_stride, col_stride>& R) { + const T* euler, const MatrixAdapter<T, row_stride, col_stride>& R) { const double kPi = 3.14159265358979323846; const T degrees_to_radians(kPi / 180.0); @@ -438,28 +448,27 @@ void EulerAnglesToRotationMatrix( const T c3 = cos(pitch); const T s3 = sin(pitch); - R(0, 0) = c1*c2; - R(0, 1) = -s1*c3 + c1*s2*s3; - R(0, 2) = s1*s3 + c1*s2*c3; + R(0, 0) = c1 * c2; + R(0, 1) = -s1 * c3 + c1 * s2 * s3; + R(0, 2) = s1 * s3 + c1 * s2 * c3; - R(1, 0) = s1*c2; - R(1, 1) = c1*c3 + s1*s2*s3; - R(1, 2) = -c1*s3 + s1*s2*c3; + R(1, 0) = s1 * c2; + R(1, 1) = c1 * c3 + s1 * s2 * s3; + R(1, 2) = -c1 * s3 + s1 * s2 * c3; R(2, 0) = -s2; - R(2, 1) = c2*s3; - R(2, 2) = c2*c3; + R(2, 1) = c2 * s3; + R(2, 2) = c2 * c3; } -template <typename T> inline -void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { +template <typename T> +inline void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { QuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); } -template <typename T, int row_stride, int col_stride> inline -void QuaternionToScaledRotation( - const T q[4], - const MatrixAdapter<T, row_stride, col_stride>& R) { +template <typename T, int row_stride, int col_stride> +inline void QuaternionToScaledRotation( + const T q[4], const MatrixAdapter<T, row_stride, col_stride>& R) { // Make convenient names for elements of q. T a = q[0]; T b = q[1]; @@ -478,22 +487,24 @@ void QuaternionToScaledRotation( T cd = c * d; T dd = d * d; - R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT - R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT - R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT + // clang-format off + R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); + R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); + R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; + // clang-format on } -template <typename T> inline -void QuaternionToRotation(const T q[4], T R[3 * 3]) { +template <typename T> +inline void QuaternionToRotation(const T q[4], T R[3 * 3]) { QuaternionToRotation(q, RowMajorAdapter3x3(R)); } -template <typename T, int row_stride, int col_stride> inline -void QuaternionToRotation(const T q[4], - const MatrixAdapter<T, row_stride, col_stride>& R) { +template <typename T, int row_stride, int col_stride> +inline void QuaternionToRotation( + const T q[4], const MatrixAdapter<T, row_stride, col_stride>& R) { QuaternionToScaledRotation(q, R); - T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; + T normalizer = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]; normalizer = T(1) / normalizer; for (int i = 0; i < 3; ++i) { @@ -503,8 +514,13 @@ void QuaternionToRotation(const T q[4], } } -template <typename T> inline -void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { +template <typename T> +inline void UnitQuaternionRotatePoint(const T q[4], + const T pt[3], + T result[3]) { + DCHECK_NE(pt, result) << "Inplace rotation is not supported."; + + // clang-format off const T t2 = q[0] * q[1]; const T t3 = q[0] * q[2]; const T t4 = q[0] * q[3]; @@ -517,50 +533,63 @@ void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT + // clang-format on } -template <typename T> inline -void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { +template <typename T> +inline void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + DCHECK_NE(pt, result) << "Inplace rotation is not supported."; + // 'scale' is 1 / norm(q). - const T scale = T(1) / sqrt(q[0] * q[0] + - q[1] * q[1] + - q[2] * q[2] + - q[3] * q[3]); + const T scale = + T(1) / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); // Make unit-norm version of q. const T unit[4] = { - scale * q[0], - scale * q[1], - scale * q[2], - scale * q[3], + scale * q[0], + scale * q[1], + scale * q[2], + scale * q[3], }; UnitQuaternionRotatePoint(unit, pt, result); } -template<typename T> inline -void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { +template <typename T> +inline void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { + DCHECK_NE(z, zw) << "Inplace quaternion product is not supported."; + DCHECK_NE(w, zw) << "Inplace quaternion product is not supported."; + + // clang-format off zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; + // clang-format on } // xy = x cross y; -template<typename T> inline -void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { +template <typename T> +inline void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { + DCHECK_NE(x, x_cross_y) << "Inplace cross product is not supported."; + DCHECK_NE(y, x_cross_y) << "Inplace cross product is not supported."; + x_cross_y[0] = x[1] * y[2] - x[2] * y[1]; x_cross_y[1] = x[2] * y[0] - x[0] * y[2]; x_cross_y[2] = x[0] * y[1] - x[1] * y[0]; } -template<typename T> inline -T DotProduct(const T x[3], const T y[3]) { +template <typename T> +inline T DotProduct(const T x[3], const T y[3]) { return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]); } -template<typename T> inline -void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { +template <typename T> +inline void AngleAxisRotatePoint(const T angle_axis[3], + const T pt[3], + T result[3]) { + DCHECK_NE(pt, result) << "Inplace rotation is not supported."; + const T theta2 = DotProduct(angle_axis, angle_axis); if (theta2 > T(std::numeric_limits<double>::epsilon())) { // Away from zero, use the rodriguez formula @@ -576,17 +605,17 @@ void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { const T theta = sqrt(theta2); const T costheta = cos(theta); const T sintheta = sin(theta); - const T theta_inverse = 1.0 / theta; + const T theta_inverse = T(1.0) / theta; - const T w[3] = { angle_axis[0] * theta_inverse, - angle_axis[1] * theta_inverse, - angle_axis[2] * theta_inverse }; + const T w[3] = {angle_axis[0] * theta_inverse, + angle_axis[1] * theta_inverse, + angle_axis[2] * theta_inverse}; // Explicitly inlined evaluation of the cross product for // performance reasons. - const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1], - w[2] * pt[0] - w[0] * pt[2], - w[0] * pt[1] - w[1] * pt[0] }; + const T w_cross_pt[3] = {w[1] * pt[2] - w[2] * pt[1], + w[2] * pt[0] - w[0] * pt[2], + w[0] * pt[1] - w[1] * pt[0]}; const T tmp = (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta); @@ -611,9 +640,9 @@ void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { // // Explicitly inlined evaluation of the cross product for // performance reasons. - const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1], - angle_axis[2] * pt[0] - angle_axis[0] * pt[2], - angle_axis[0] * pt[1] - angle_axis[1] * pt[0] }; + const T w_cross_pt[3] = {angle_axis[1] * pt[2] - angle_axis[2] * pt[1], + angle_axis[2] * pt[0] - angle_axis[0] * pt[2], + angle_axis[0] * pt[1] - angle_axis[1] * pt[0]}; result[0] = pt[0] + w_cross_pt[0]; result[1] = pt[1] + w_cross_pt[1]; diff --git a/extern/ceres/include/ceres/sized_cost_function.h b/extern/ceres/include/ceres/sized_cost_function.h index b10421e81be..8e92f1b796c 100644 --- a/extern/ceres/include/ceres/sized_cost_function.h +++ b/extern/ceres/include/ceres/sized_cost_function.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -38,55 +38,30 @@ #ifndef CERES_PUBLIC_SIZED_COST_FUNCTION_H_ #define CERES_PUBLIC_SIZED_COST_FUNCTION_H_ -#include "ceres/types.h" #include "ceres/cost_function.h" +#include "ceres/types.h" #include "glog/logging.h" +#include "internal/parameter_dims.h" namespace ceres { -template<int kNumResiduals, - int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, - int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> +template <int kNumResiduals, int... Ns> class SizedCostFunction : public CostFunction { public: - SizedCostFunction() { - CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC) - << "Cost functions must have at least one residual block."; + static_assert(kNumResiduals > 0 || kNumResiduals == DYNAMIC, + "Cost functions must have at least one residual block."); + static_assert(internal::StaticParameterDims<Ns...>::kIsValid, + "Invalid parameter block dimension detected. Each parameter " + "block dimension must be bigger than zero."); - // This block breaks the 80 column rule to keep it somewhat readable. - CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || - ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || - ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT - << "Zero block cannot precede a non-zero block. Block sizes are " - << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " - << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " - << N8 << ", " << N9; + using ParameterDims = internal::StaticParameterDims<Ns...>; + SizedCostFunction() { set_num_residuals(kNumResiduals); - -#define CERES_ADD_PARAMETER_BLOCK(N) \ - if (N) mutable_parameter_block_sizes()->push_back(N); - CERES_ADD_PARAMETER_BLOCK(N0); - CERES_ADD_PARAMETER_BLOCK(N1); - CERES_ADD_PARAMETER_BLOCK(N2); - CERES_ADD_PARAMETER_BLOCK(N3); - CERES_ADD_PARAMETER_BLOCK(N4); - CERES_ADD_PARAMETER_BLOCK(N5); - CERES_ADD_PARAMETER_BLOCK(N6); - CERES_ADD_PARAMETER_BLOCK(N7); - CERES_ADD_PARAMETER_BLOCK(N8); - CERES_ADD_PARAMETER_BLOCK(N9); -#undef CERES_ADD_PARAMETER_BLOCK + *mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...}; } - virtual ~SizedCostFunction() { } + virtual ~SizedCostFunction() {} // Subclasses must implement Evaluate(). }; diff --git a/extern/ceres/include/ceres/solver.h b/extern/ceres/include/ceres/solver.h index 0d77d242dfe..62631744fe2 100644 --- a/extern/ceres/include/ceres/solver.h +++ b/extern/ceres/include/ceres/solver.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -32,20 +32,21 @@ #define CERES_PUBLIC_SOLVER_H_ #include <cmath> +#include <memory> #include <string> +#include <unordered_set> #include <vector> + #include "ceres/crs_matrix.h" -#include "ceres/internal/macros.h" +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/port.h" #include "ceres/iteration_callback.h" #include "ceres/ordered_groups.h" +#include "ceres/problem.h" #include "ceres/types.h" -#include "ceres/internal/disable_warnings.h" namespace ceres { -class Problem; - // Interface for non-linear least squares solvers. class CERES_EXPORT Solver { public: @@ -57,87 +58,6 @@ class CERES_EXPORT Solver { // // The constants are defined inside types.h struct CERES_EXPORT Options { - // Default constructor that sets up a generic sparse problem. - Options() { - minimizer_type = TRUST_REGION; - line_search_direction_type = LBFGS; - line_search_type = WOLFE; - nonlinear_conjugate_gradient_type = FLETCHER_REEVES; - max_lbfgs_rank = 20; - use_approximate_eigenvalue_bfgs_scaling = false; - line_search_interpolation_type = CUBIC; - min_line_search_step_size = 1e-9; - line_search_sufficient_function_decrease = 1e-4; - max_line_search_step_contraction = 1e-3; - min_line_search_step_contraction = 0.6; - max_num_line_search_step_size_iterations = 20; - max_num_line_search_direction_restarts = 5; - line_search_sufficient_curvature_decrease = 0.9; - max_line_search_step_expansion = 10.0; - trust_region_strategy_type = LEVENBERG_MARQUARDT; - dogleg_type = TRADITIONAL_DOGLEG; - use_nonmonotonic_steps = false; - max_consecutive_nonmonotonic_steps = 5; - max_num_iterations = 50; - max_solver_time_in_seconds = 1e9; - num_threads = 1; - initial_trust_region_radius = 1e4; - max_trust_region_radius = 1e16; - min_trust_region_radius = 1e-32; - min_relative_decrease = 1e-3; - min_lm_diagonal = 1e-6; - max_lm_diagonal = 1e32; - max_num_consecutive_invalid_steps = 5; - function_tolerance = 1e-6; - gradient_tolerance = 1e-10; - parameter_tolerance = 1e-8; - -#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) && !defined(CERES_ENABLE_LGPL_CODE) // NOLINT - linear_solver_type = DENSE_QR; -#else - linear_solver_type = SPARSE_NORMAL_CHOLESKY; -#endif - - preconditioner_type = JACOBI; - visibility_clustering_type = CANONICAL_VIEWS; - dense_linear_algebra_library_type = EIGEN; - - // Choose a default sparse linear algebra library in the order: - // - // SUITE_SPARSE > CX_SPARSE > EIGEN_SPARSE > NO_SPARSE - sparse_linear_algebra_library_type = NO_SPARSE; -#if !defined(CERES_NO_SUITESPARSE) - sparse_linear_algebra_library_type = SUITE_SPARSE; -#else - #if !defined(CERES_NO_CXSPARSE) - sparse_linear_algebra_library_type = CX_SPARSE; - #else - #if defined(CERES_USE_EIGEN_SPARSE) - sparse_linear_algebra_library_type = EIGEN_SPARSE; - #endif - #endif -#endif - - num_linear_solver_threads = 1; - use_explicit_schur_complement = false; - use_postordering = false; - dynamic_sparsity = false; - min_linear_solver_iterations = 0; - max_linear_solver_iterations = 500; - eta = 1e-1; - jacobi_scaling = true; - use_inner_iterations = false; - inner_iteration_tolerance = 1e-3; - logging_type = PER_MINIMIZER_ITERATION; - minimizer_progress_to_stdout = false; - trust_region_problem_dump_directory = "/tmp"; - trust_region_problem_dump_format_type = TEXTFILE; - check_gradients = false; - gradient_check_relative_precision = 1e-8; - gradient_check_numeric_derivative_relative_step_size = 1e-6; - update_state_every_iteration = false; - } - // Returns true if the options struct has a valid // configuration. Returns false otherwise, and fills in *error // with a message describing the problem. @@ -157,7 +77,7 @@ class CERES_EXPORT Solver { // exactly or inexactly. // // 2. The trust region approach approximates the objective - // function using using a model function (often a quadratic) over + // function using a model function (often a quadratic) over // a subset of the search space known as the trust region. If the // model function succeeds in minimizing the true objective // function the trust region is expanded; conversely, otherwise it @@ -168,11 +88,12 @@ class CERES_EXPORT Solver { // trust region methods first choose a step size (the size of the // trust region) and then a step direction while line search methods // first choose a step direction and then a step size. - MinimizerType minimizer_type; + MinimizerType minimizer_type = TRUST_REGION; - LineSearchDirectionType line_search_direction_type; - LineSearchType line_search_type; - NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; + LineSearchDirectionType line_search_direction_type = LBFGS; + LineSearchType line_search_type = WOLFE; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type = + FLETCHER_REEVES; // The LBFGS hessian approximation is a low rank approximation to // the inverse of the Hessian matrix. The rank of the @@ -197,8 +118,8 @@ class CERES_EXPORT Solver { // method, please see: // // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with - // Limited Storage". Mathematics of Computation 35 (151): 773–782. - int max_lbfgs_rank; + // Limited Storage". Mathematics of Computation 35 (151): 773-782. + int max_lbfgs_rank = 20; // As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS), // the initial inverse Hessian approximation is taken to be the Identity. @@ -220,18 +141,18 @@ class CERES_EXPORT Solver { // Oren S.S., Self-scaling variable metric (SSVM) algorithms // Part II: Implementation and experiments, Management Science, // 20(5), 863-874, 1974. - bool use_approximate_eigenvalue_bfgs_scaling; + bool use_approximate_eigenvalue_bfgs_scaling = false; // Degree of the polynomial used to approximate the objective // function. Valid values are BISECTION, QUADRATIC and CUBIC. // // BISECTION corresponds to pure backtracking search with no // interpolation. - LineSearchInterpolationType line_search_interpolation_type; + LineSearchInterpolationType line_search_interpolation_type = CUBIC; // If during the line search, the step_size falls below this // value, it is truncated to zero. - double min_line_search_step_size; + double min_line_search_step_size = 1e-9; // Line search parameters. @@ -245,7 +166,7 @@ class CERES_EXPORT Solver { // // f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size // - double line_search_sufficient_function_decrease; + double line_search_sufficient_function_decrease = 1e-4; // In each iteration of the line search, // @@ -255,7 +176,7 @@ class CERES_EXPORT Solver { // // 0 < max_step_contraction < min_step_contraction < 1 // - double max_line_search_step_contraction; + double max_line_search_step_contraction = 1e-3; // In each iteration of the line search, // @@ -265,19 +186,25 @@ class CERES_EXPORT Solver { // // 0 < max_step_contraction < min_step_contraction < 1 // - double min_line_search_step_contraction; + double min_line_search_step_contraction = 0.6; - // Maximum number of trial step size iterations during each line search, - // if a step size satisfying the search conditions cannot be found within - // this number of trials, the line search will terminate. - int max_num_line_search_step_size_iterations; + // Maximum number of trial step size iterations during each line + // search, if a step size satisfying the search conditions cannot + // be found within this number of trials, the line search will + // terminate. + + // The minimum allowed value is 0 for trust region minimizer and 1 + // otherwise. If 0 is specified for the trust region minimizer, + // then line search will not be used when solving constrained + // optimization problems. + int max_num_line_search_step_size_iterations = 20; // Maximum number of restarts of the line search direction algorithm before // terminating the optimization. Restarts of the line search direction // algorithm occur when the current algorithm fails to produce a new descent // direction. This typically indicates a numerical failure, or a breakdown // in the validity of the approximations used. - int max_num_line_search_direction_restarts; + int max_num_line_search_direction_restarts = 5; // The strong Wolfe conditions consist of the Armijo sufficient // decrease condition, and an additional requirement that the @@ -290,7 +217,7 @@ class CERES_EXPORT Solver { // // Where f() is the line search objective and f'() is the derivative // of f w.r.t step_size (d f / d step_size). - double line_search_sufficient_curvature_decrease; + double line_search_sufficient_curvature_decrease = 0.9; // During the bracketing phase of the Wolfe search, the step size is // increased until either a point satisfying the Wolfe conditions is @@ -301,12 +228,12 @@ class CERES_EXPORT Solver { // new_step_size <= max_step_expansion * step_size. // // By definition for expansion, max_step_expansion > 1.0. - double max_line_search_step_expansion; + double max_line_search_step_expansion = 10.0; - TrustRegionStrategyType trust_region_strategy_type; + TrustRegionStrategyType trust_region_strategy_type = LEVENBERG_MARQUARDT; // Type of dogleg strategy to use. - DoglegType dogleg_type; + DoglegType dogleg_type = TRADITIONAL_DOGLEG; // The classical trust region methods are descent methods, in that // they only accept a point if it strictly reduces the value of @@ -317,7 +244,7 @@ class CERES_EXPORT Solver { // in the value of the objective function. // // This is because allowing for non-decreasing objective function - // values in a princpled manner allows the algorithm to "jump over + // 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. // @@ -333,30 +260,30 @@ class CERES_EXPORT Solver { // than the minimum value encountered over the course of the // optimization, the final parameters returned to the user are the // ones corresponding to the minimum cost over all iterations. - bool use_nonmonotonic_steps; - int max_consecutive_nonmonotonic_steps; + bool use_nonmonotonic_steps = false; + int max_consecutive_nonmonotonic_steps = 5; // Maximum number of iterations for the minimizer to run for. - int max_num_iterations; + int max_num_iterations = 50; // Maximum time for which the minimizer should run for. - double max_solver_time_in_seconds; + double max_solver_time_in_seconds = 1e9; // Number of threads used by Ceres for evaluating the cost and // jacobians. - int num_threads; + int num_threads = 1; // Trust region minimizer settings. - double initial_trust_region_radius; - double max_trust_region_radius; + double initial_trust_region_radius = 1e4; + double max_trust_region_radius = 1e16; // Minimizer terminates when the trust region radius becomes // smaller than this value. - double min_trust_region_radius; + double min_trust_region_radius = 1e-32; // Lower bound for the relative decrease before a step is // accepted. - double min_relative_decrease; + double min_relative_decrease = 1e-3; // For the Levenberg-Marquadt algorithm, the scaled diagonal of // the normal equations J'J is used to control the size of the @@ -365,46 +292,75 @@ class CERES_EXPORT Solver { // fail. max_lm_diagonal and min_lm_diagonal, clamp the values of // diag(J'J) from above and below. In the normal course of // operation, the user should not have to modify these parameters. - double min_lm_diagonal; - double max_lm_diagonal; + double min_lm_diagonal = 1e-6; + double max_lm_diagonal = 1e32; // Sometimes due to numerical conditioning problems or linear // solver flakiness, the trust region strategy may return a // numerically invalid step that can be fixed by reducing the // trust region size. So the TrustRegionMinimizer allows for a few // successive invalid steps before it declares NUMERICAL_FAILURE. - int max_num_consecutive_invalid_steps; + int max_num_consecutive_invalid_steps = 5; // Minimizer terminates when // // (new_cost - old_cost) < function_tolerance * old_cost; // - double function_tolerance; + double function_tolerance = 1e-6; // Minimizer terminates when // // max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance // // This value should typically be 1e-4 * function_tolerance. - double gradient_tolerance; + double gradient_tolerance = 1e-10; // Minimizer terminates when // // |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance) // - double parameter_tolerance; + double parameter_tolerance = 1e-8; // Linear least squares solver options ------------------------------------- - LinearSolverType linear_solver_type; + LinearSolverType linear_solver_type = +#if defined(CERES_NO_SPARSE) + DENSE_QR; +#else + SPARSE_NORMAL_CHOLESKY; +#endif // Type of preconditioner to use with the iterative linear solvers. - PreconditionerType preconditioner_type; + PreconditionerType preconditioner_type = JACOBI; // Type of clustering algorithm to use for visibility based // preconditioning. This option is used only when the // preconditioner_type is CLUSTER_JACOBI or CLUSTER_TRIDIAGONAL. - VisibilityClusteringType visibility_clustering_type; + VisibilityClusteringType visibility_clustering_type = CANONICAL_VIEWS; + + // Subset preconditioner is a preconditioner for problems with + // general sparsity. Given a subset of residual blocks of a + // problem, it uses the corresponding subset of the rows of the + // Jacobian to construct a preconditioner. + // + // Suppose the Jacobian J has been horizontally partitioned as + // + // J = [P] + // [Q] + // + // Where, Q is the set of rows corresponding to the residual + // blocks in residual_blocks_for_subset_preconditioner. + // + // The preconditioner is the inverse of the matrix Q'Q. + // + // Obviously, the efficacy of the preconditioner depends on how + // well the matrix Q approximates J'J, or how well the chosen + // residual blocks approximate the non-linear least squares + // problem. + // + // If Solver::Options::preconditioner_type == SUBSET, then + // residual_blocks_for_subset_preconditioner must be non-empty. + std::unordered_set<ResidualBlockId> residual_blocks_for_subset_preconditioner; // Ceres supports using multiple dense linear algebra libraries // for dense matrix factorizations. Currently EIGEN and LAPACK are @@ -413,22 +369,28 @@ class CERES_EXPORT Solver { // available. // // This setting affects the DENSE_QR, DENSE_NORMAL_CHOLESKY and - // DENSE_SCHUR solvers. For small to moderate sized probem EIGEN + // DENSE_SCHUR solvers. For small to moderate sized problem EIGEN // is a fine choice but for large problems, an optimized LAPACK + // BLAS implementation can make a substantial difference in // performance. - DenseLinearAlgebraLibraryType dense_linear_algebra_library_type; + DenseLinearAlgebraLibraryType dense_linear_algebra_library_type = EIGEN; // Ceres supports using multiple sparse linear algebra libraries // for sparse matrix ordering and factorizations. Currently, // SUITE_SPARSE and CX_SPARSE are the valid choices, depending on // whether they are linked into Ceres at build time. - SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; - - // Number of threads used by Ceres to solve the Newton - // step. Currently only the SPARSE_SCHUR solver is capable of - // using this setting. - int num_linear_solver_threads; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type = +#if !defined(CERES_NO_SUITESPARSE) + SUITE_SPARSE; +#elif defined(CERES_USE_EIGEN_SPARSE) + EIGEN_SPARSE; +#elif !defined(CERES_NO_CXSPARSE) + CX_SPARSE; +#elif !defined(CERES_NO_ACCELERATE_SPARSE) + ACCELERATE_SPARSE; +#else + NO_SPARSE; +#endif // The order in which variables are eliminated in a linear solver // can have a significant of impact on the efficiency and accuracy @@ -456,7 +418,7 @@ class CERES_EXPORT Solver { // // Given such an ordering, Ceres ensures that the parameter blocks in // the lowest numbered group are eliminated first, and then the - // parmeter blocks in the next lowest numbered group and so on. Within + // parameter blocks in the next lowest numbered group and so on. Within // each group, Ceres is free to order the parameter blocks as it // chooses. // @@ -496,13 +458,13 @@ class CERES_EXPORT Solver { // the parameter blocks into two groups, one for the points and one // for the cameras, where the group containing the points has an id // smaller than the group containing cameras. - shared_ptr<ParameterBlockOrdering> linear_solver_ordering; + std::shared_ptr<ParameterBlockOrdering> linear_solver_ordering; // Use an explicitly computed Schur complement matrix with // ITERATIVE_SCHUR. // // By default this option is disabled and ITERATIVE_SCHUR - // evaluates evaluates matrix-vector products between the Schur + // evaluates matrix-vector products between the Schur // complement and a vector implicitly by exploiting the algebraic // expression for the Schur complement. // @@ -519,7 +481,7 @@ class CERES_EXPORT Solver { // // NOTE: This option can only be used with the SCHUR_JACOBI // preconditioner. - bool use_explicit_schur_complement; + bool use_explicit_schur_complement = false; // Sparse Cholesky factorization algorithms use a fill-reducing // ordering to permute the columns of the Jacobian matrix. There @@ -540,7 +502,7 @@ class CERES_EXPORT Solver { // reordering algorithm which has slightly better runtime // performance at the expense of an extra copy of the Jacobian // matrix. Setting use_postordering to true enables this tradeoff. - bool use_postordering; + bool use_postordering = false; // Some non-linear least squares problems are symbolically dense but // numerically sparse. i.e. at any given state only a small number @@ -554,8 +516,32 @@ class CERES_EXPORT Solver { // then it is probably best to keep this false, otherwise it will // likely lead to worse performance. - // This settings affects the SPARSE_NORMAL_CHOLESKY solver. - bool dynamic_sparsity; + // This settings only affects the SPARSE_NORMAL_CHOLESKY solver. + bool dynamic_sparsity = false; + + // TODO(sameeragarwal): Further expand the documentation for the + // following two options. + + // NOTE1: EXPERIMENTAL FEATURE, UNDER DEVELOPMENT, USE AT YOUR OWN RISK. + // + // If use_mixed_precision_solves is true, the Gauss-Newton matrix + // is computed in double precision, but its factorization is + // computed in single precision. This can result in significant + // time and memory savings at the cost of some accuracy in the + // Gauss-Newton step. Iterative refinement is used to recover some + // of this accuracy back. + // + // If use_mixed_precision_solves is true, we recommend setting + // max_num_refinement_iterations to 2-3. + // + // NOTE2: The following two options are currently only applicable + // if sparse_linear_algebra_library_type is EIGEN_SPARSE and + // linear_solver_type is SPARSE_NORMAL_CHOLESKY, or SPARSE_SCHUR. + bool use_mixed_precision_solves = false; + + // Number steps of the iterative refinement process to run when + // computing the Gauss-Newton step. + int max_num_refinement_iterations = 0; // Some non-linear least squares problems have additional // structure in the way the parameter blocks interact that it is @@ -583,7 +569,7 @@ class CERES_EXPORT Solver { // known as Wiberg's algorithm. // // Ruhe & Wedin (Algorithms for Separable Nonlinear Least Squares - // Problems, SIAM Reviews, 22(3), 1980) present an analyis of + // Problems, SIAM Reviews, 22(3), 1980) present an analysis of // various algorithms for solving separable non-linear least // squares problems and refer to "Variable Projection" as // Algorithm I in their paper. @@ -615,7 +601,7 @@ class CERES_EXPORT Solver { // displays better convergence behaviour per iteration. Setting // Solver::Options::num_threads to the maximum number possible is // highly recommended. - bool use_inner_iterations; + bool use_inner_iterations = false; // If inner_iterations is true, then the user has two choices. // @@ -627,7 +613,7 @@ class CERES_EXPORT Solver { // the lower numbered groups are optimized before the higher // number groups. Each group must be an independent set. Not // all parameter blocks need to be present in the ordering. - shared_ptr<ParameterBlockOrdering> inner_iteration_ordering; + std::shared_ptr<ParameterBlockOrdering> inner_iteration_ordering; // Generally speaking, inner iterations make significant progress // in the early stages of the solve and then their contribution @@ -638,17 +624,17 @@ class CERES_EXPORT Solver { // inner iterations drops below inner_iteration_tolerance, the use // of inner iterations in subsequent trust region minimizer // iterations is disabled. - double inner_iteration_tolerance; + double inner_iteration_tolerance = 1e-3; // Minimum number of iterations for which the linear solver should // run, even if the convergence criterion is satisfied. - int min_linear_solver_iterations; + int min_linear_solver_iterations = 0; // Maximum number of iterations for which the linear solver should // run. If the solver does not converge in less than // max_linear_solver_iterations, then it returns MAX_ITERATIONS, // as its termination type. - int max_linear_solver_iterations; + int max_linear_solver_iterations = 500; // Forcing sequence parameter. The truncated Newton solver uses // this number to control the relative accuracy with which the @@ -658,21 +644,21 @@ class CERES_EXPORT Solver { // it to terminate the iterations when // // (Q_i - Q_{i-1})/Q_i < eta/i - double eta; + double eta = 1e-1; // Normalize the jacobian using Jacobi scaling before calling // the linear least squares solver. - bool jacobi_scaling; + bool jacobi_scaling = true; // Logging options --------------------------------------------------------- - LoggingType logging_type; + LoggingType logging_type = PER_MINIMIZER_ITERATION; // By default the Minimizer progress is logged to VLOG(1), which // is sent to STDERR depending on the vlog level. If this flag is // set to true, and logging_type is not SILENT, the logging output // is sent to STDOUT. - bool minimizer_progress_to_stdout; + bool minimizer_progress_to_stdout = false; // List of iterations at which the minimizer should dump the trust // region problem. Useful for testing and benchmarking. If empty @@ -683,8 +669,8 @@ class CERES_EXPORT Solver { // non-empty if trust_region_minimizer_iterations_to_dump is // non-empty and trust_region_problem_dump_format_type is not // CONSOLE. - std::string trust_region_problem_dump_directory; - DumpFormatType trust_region_problem_dump_format_type; + std::string trust_region_problem_dump_directory = "/tmp"; + DumpFormatType trust_region_problem_dump_format_type = TEXTFILE; // Finite differences options ---------------------------------------------- @@ -694,12 +680,12 @@ class CERES_EXPORT Solver { // etc), then also computing it using finite differences. The // results are compared, and if they differ substantially, details // are printed to the log. - bool check_gradients; + bool check_gradients = false; // Relative precision to check for in the gradient checker. If the // relative difference between an element in a jacobian exceeds // this number, then the jacobian for that cost term is dumped. - double gradient_check_relative_precision; + double gradient_check_relative_precision = 1e-8; // WARNING: This option only applies to the to the numeric // differentiation used for checking the user provided derivatives @@ -723,7 +709,7 @@ class CERES_EXPORT Solver { // // The finite differencing is done along each dimension. The // reason to use a relative (rather than absolute) step size is - // that this way, numeric differentation works for functions where + // that this way, numeric differentiation works for functions where // the arguments are typically large (e.g. 1e9) and when the // values are small (e.g. 1e-5). It is possible to construct // "torture cases" which break this finite difference heuristic, @@ -733,14 +719,21 @@ 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 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 - // Minimizer terminates. This is useful if, for example, the user - // wishes to visualize the state of the optimization every - // iteration. - bool update_state_every_iteration; + double gradient_check_numeric_derivative_relative_step_size = 1e-6; + + // If update_state_every_iteration is true, then Ceres Solver will + // guarantee that at the end of every iteration and before any + // user provided IterationCallback is called, the parameter blocks + // are updated to the current best solution found by the + // solver. Thus the IterationCallback can inspect the values of + // the parameter blocks for purposes of computation, visualization + // or termination. + + // If update_state_every_iteration is false then there is no such + // guarantee, and user provided IterationCallbacks should not + // expect to look at the parameter blocks and interpret their + // values. + bool update_state_every_iteration = false; // Callbacks that are executed at the end of each iteration of the // Minimizer. An iteration may terminate midway, either due to @@ -749,20 +742,18 @@ class CERES_EXPORT Solver { // executed. // Callbacks are executed in the order that they are specified in - // this vector. By default, parameter blocks are updated only at - // the end of the optimization, i.e when the Minimizer - // terminates. This behaviour is controlled by - // update_state_every_variable. If the user wishes to have access - // to the update parameter blocks when his/her callbacks are - // executed, then set update_state_every_iteration to true. + // this vector. By default, parameter blocks are updated only at the + // end of the optimization, i.e when the Minimizer terminates. This + // behaviour is controlled by update_state_every_iteration. If the + // user wishes to have access to the updated parameter blocks when + // his/her callbacks are executed, then set + // update_state_every_iteration to true. // // The solver does NOT take ownership of these pointers. std::vector<IterationCallback*> callbacks; }; struct CERES_EXPORT Summary { - Summary(); - // A brief one line description of the state of the solver after // termination. std::string BriefReport() const; @@ -774,25 +765,25 @@ class CERES_EXPORT Solver { bool IsSolutionUsable() const; // Minimizer summary ------------------------------------------------- - MinimizerType minimizer_type; + MinimizerType minimizer_type = TRUST_REGION; - TerminationType termination_type; + TerminationType termination_type = FAILURE; // Reason why the solver terminated. - std::string message; + std::string message = "ceres::Solve was not called."; // Cost of the problem (value of the objective function) before // the optimization. - double initial_cost; + double initial_cost = -1.0; // Cost of the problem (value of the objective function) after the // optimization. - double final_cost; + double final_cost = -1.0; // The part of the total cost that comes from residual blocks that // were held fixed by the preprocessor because all the parameter // blocks that they depend on were fixed. - double fixed_cost; + double fixed_cost = -1.0; // IterationSummary for each minimizer iteration in order. std::vector<IterationSummary> iterations; @@ -801,22 +792,22 @@ class CERES_EXPORT Solver { // accepted. Unless use_non_monotonic_steps is true this is also // the number of steps in which the objective function value/cost // went down. - int num_successful_steps; + int num_successful_steps = -1; // Number of minimizer iterations in which the step was rejected // either because it did not reduce the cost enough or the step // was not numerically valid. - int num_unsuccessful_steps; + int num_unsuccessful_steps = -1; // Number of times inner iterations were performed. - int num_inner_iteration_steps; + int num_inner_iteration_steps = -1; // 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; + int num_line_search_steps = -1; // All times reported below are wall times. @@ -824,31 +815,42 @@ class CERES_EXPORT Solver { // occurs, Ceres performs a number of preprocessing steps. These // include error checks, memory allocations, and reorderings. This // time is accounted for as preprocessing time. - double preprocessor_time_in_seconds; + double preprocessor_time_in_seconds = -1.0; // Time spent in the TrustRegionMinimizer. - double minimizer_time_in_seconds; + double minimizer_time_in_seconds = -1.0; // After the Minimizer is finished, some time is spent in // re-evaluating residuals etc. This time is accounted for in the // postprocessor time. - double postprocessor_time_in_seconds; + double postprocessor_time_in_seconds = -1.0; // Some total of all time spent inside Ceres when Solve is called. - double total_time_in_seconds; + double total_time_in_seconds = -1.0; // Time (in seconds) spent in the linear solver computing the // trust region step. - double linear_solver_time_in_seconds; + double linear_solver_time_in_seconds = -1.0; + + // Number of times the Newton step was computed by solving a + // linear system. This does not include linear solves used by + // inner iterations. + int num_linear_solves = -1; // Time (in seconds) spent evaluating the residual vector. - double residual_evaluation_time_in_seconds; + double residual_evaluation_time_in_seconds = 1.0; + + // Number of residual only evaluations. + int num_residual_evaluations = -1; // Time (in seconds) spent evaluating the jacobian matrix. - double jacobian_evaluation_time_in_seconds; + double jacobian_evaluation_time_in_seconds = -1.0; + + // Number of Jacobian (and residual) evaluations. + int num_jacobian_evaluations = -1; // Time (in seconds) spent doing inner iterations. - double inner_iteration_time_in_seconds; + double inner_iteration_time_in_seconds = -1.0; // Cumulative timing information for line searches performed as part of the // solve. Note that in addition to the case when the Line Search minimizer @@ -857,89 +859,89 @@ class CERES_EXPORT Solver { // Time (in seconds) spent evaluating the univariate cost function as part // of a line search. - double line_search_cost_evaluation_time_in_seconds; + double line_search_cost_evaluation_time_in_seconds = -1.0; // Time (in seconds) spent evaluating the gradient of the univariate cost // function as part of a line search. - double line_search_gradient_evaluation_time_in_seconds; + double line_search_gradient_evaluation_time_in_seconds = -1.0; // Time (in seconds) spent minimizing the interpolating polynomial // to compute the next candidate step size as part of a line search. - double line_search_polynomial_minimization_time_in_seconds; + double line_search_polynomial_minimization_time_in_seconds = -1.0; // Total time (in seconds) spent performing line searches. - double line_search_total_time_in_seconds; + double line_search_total_time_in_seconds = -1.0; // Number of parameter blocks in the problem. - int num_parameter_blocks; + int num_parameter_blocks = -1; - // Number of parameters in the probem. - int num_parameters; + // Number of parameters in the problem. + int num_parameters = -1; // Dimension of the tangent space of the problem (or the number of // columns in the Jacobian for the problem). This is different // from num_parameters if a parameter block is associated with a // LocalParameterization - int num_effective_parameters; + int num_effective_parameters = -1; // Number of residual blocks in the problem. - int num_residual_blocks; + int num_residual_blocks = -1; // Number of residuals in the problem. - int num_residuals; + int num_residuals = -1; // Number of parameter blocks in the problem after the inactive // and constant parameter blocks have been removed. A parameter // block is inactive if no residual block refers to it. - int num_parameter_blocks_reduced; + int num_parameter_blocks_reduced = -1; // Number of parameters in the reduced problem. - int num_parameters_reduced; + int num_parameters_reduced = -1; // Dimension of the tangent space of the reduced problem (or the // number of columns in the Jacobian for the reduced // problem). This is different from num_parameters_reduced if a // parameter block in the reduced problem is associated with a // LocalParameterization. - int num_effective_parameters_reduced; + int num_effective_parameters_reduced = -1; // Number of residual blocks in the reduced problem. - int num_residual_blocks_reduced; + int num_residual_blocks_reduced = -1; // Number of residuals in the reduced problem. - int num_residuals_reduced; + int num_residuals_reduced = -1; // Is the reduced problem bounds constrained. - bool is_constrained; + bool is_constrained = false; // Number of threads specified by the user for Jacobian and // residual evaluation. - int num_threads_given; + int num_threads_given = -1; // Number of threads actually used by the solver for Jacobian and // residual evaluation. This number is not equal to // num_threads_given if OpenMP is not available. - int num_threads_used; - - // Number of threads specified by the user for solving the trust - // region problem. - int num_linear_solver_threads_given; - - // Number of threads actually used by the solver for solving the - // trust region problem. This number is not equal to - // num_threads_given if OpenMP is not available. - int num_linear_solver_threads_used; + int num_threads_used = -1; // Type of the linear solver requested by the user. - LinearSolverType linear_solver_type_given; - + LinearSolverType linear_solver_type_given = +#if defined(CERES_NO_SPARSE) + DENSE_QR; +#else + SPARSE_NORMAL_CHOLESKY; +#endif // Type of the linear solver actually used. This may be different // from linear_solver_type_given if Ceres determines that the // problem structure is not compatible with the linear solver // requested or if the linear solver requested by the user is not // available, e.g. The user requested SPARSE_NORMAL_CHOLESKY but // no sparse linear algebra library was available. - LinearSolverType linear_solver_type_used; + LinearSolverType linear_solver_type_used = +#if defined(CERES_NO_SPARSE) + DENSE_QR; +#else + SPARSE_NORMAL_CHOLESKY; +#endif // Size of the elimination groups given by the user as hints to // the linear solver. @@ -953,15 +955,29 @@ class CERES_EXPORT Solver { // parameter blocks. std::vector<int> linear_solver_ordering_used; + // For Schur type linear solvers, this string describes the + // template specialization which was detected in the problem and + // should be used. + std::string schur_structure_given; + + // This is the Schur template specialization that was actually + // instantiated and used. The reason this will be different from + // schur_structure_given is because the corresponding template + // specialization does not exist. + // + // Template specializations can be added to ceres by editing + // internal/ceres/generate_template_specializations.py + std::string schur_structure_used; + // True if the user asked for inner iterations to be used as part // of the optimization. - bool inner_iterations_given; + bool inner_iterations_given = false; // True if the user asked for inner iterations to be used as part // of the optimization and the problem structure was such that // they were actually performed. e.g., in a problem with just one // parameter block, inner iterations are not performed. - bool inner_iterations_used; + bool inner_iterations_used = false; // Size of the parameter groups given by the user for performing // inner iterations. @@ -976,57 +992,59 @@ class CERES_EXPORT Solver { std::vector<int> inner_iteration_ordering_used; // Type of the preconditioner requested by the user. - PreconditionerType preconditioner_type_given; + PreconditionerType preconditioner_type_given = IDENTITY; // Type of the preconditioner actually used. This may be different // from linear_solver_type_given if Ceres determines that the // problem structure is not compatible with the linear solver // requested or if the linear solver requested by the user is not // available. - PreconditionerType preconditioner_type_used; + PreconditionerType preconditioner_type_used = IDENTITY; // Type of clustering algorithm used for visibility based // preconditioning. Only meaningful when the preconditioner_type // is CLUSTER_JACOBI or CLUSTER_TRIDIAGONAL. - VisibilityClusteringType visibility_clustering_type; + VisibilityClusteringType visibility_clustering_type = CANONICAL_VIEWS; // Type of trust region strategy. - TrustRegionStrategyType trust_region_strategy_type; + TrustRegionStrategyType trust_region_strategy_type = LEVENBERG_MARQUARDT; // Type of dogleg strategy used for solving the trust region // problem. - DoglegType dogleg_type; + DoglegType dogleg_type = TRADITIONAL_DOGLEG; // Type of the dense linear algebra library used. - DenseLinearAlgebraLibraryType dense_linear_algebra_library_type; + DenseLinearAlgebraLibraryType dense_linear_algebra_library_type = EIGEN; // Type of the sparse linear algebra library used. - SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type = + NO_SPARSE; // Type of line search direction used. - LineSearchDirectionType line_search_direction_type; + LineSearchDirectionType line_search_direction_type = LBFGS; // Type of the line search algorithm used. - LineSearchType line_search_type; + LineSearchType line_search_type = WOLFE; // When performing line search, the degree of the polynomial used // to approximate the objective function. - LineSearchInterpolationType line_search_interpolation_type; + LineSearchInterpolationType line_search_interpolation_type = CUBIC; // If the line search direction is NONLINEAR_CONJUGATE_GRADIENT, // then this indicates the particular variant of non-linear // conjugate gradient used. - NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type = + FLETCHER_REEVES; // If the type of the line search direction is LBFGS, then this // indicates the rank of the Hessian approximation. - int max_lbfgs_rank; + int max_lbfgs_rank = -1; }; // Once a least squares problem has been built, this function takes // the problem and optimizes it based on the values of the options // parameters. Upon return, a detailed summary of the work performed - // by the preprocessor, the non-linear minmizer and the linear + // by the preprocessor, the non-linear minimizer and the linear // solver are reported in the summary object. virtual void Solve(const Options& options, Problem* problem, @@ -1035,8 +1053,8 @@ class CERES_EXPORT Solver { // Helper function which avoids going through the interface. CERES_EXPORT void Solve(const Solver::Options& options, - Problem* problem, - Solver::Summary* summary); + Problem* problem, + Solver::Summary* summary); } // namespace ceres diff --git a/extern/ceres/include/ceres/tiny_solver.h b/extern/ceres/include/ceres/tiny_solver.h new file mode 100644 index 00000000000..47db5824dc5 --- /dev/null +++ b/extern/ceres/include/ceres/tiny_solver.h @@ -0,0 +1,368 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: mierle@gmail.com (Keir Mierle) +// +// WARNING WARNING WARNING +// WARNING WARNING WARNING Tiny solver is experimental and will change. +// WARNING WARNING WARNING +// +// A tiny least squares solver using Levenberg-Marquardt, intended for solving +// small dense problems with low latency and low overhead. The implementation +// takes care to do all allocation up front, so that no memory is allocated +// during solving. This is especially useful when solving many similar problems; +// for example, inverse pixel distortion for every pixel on a grid. +// +// Note: This code has no dependencies beyond Eigen, including on other parts of +// Ceres, so it is possible to take this file alone and put it in another +// project without the rest of Ceres. +// +// Algorithm based off of: +// +// [1] K. Madsen, H. Nielsen, O. Tingleoff. +// Methods for Non-linear Least Squares Problems. +// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf + +#ifndef CERES_PUBLIC_TINY_SOLVER_H_ +#define CERES_PUBLIC_TINY_SOLVER_H_ + +#include <cassert> +#include <cmath> + +#include "Eigen/Dense" + +namespace ceres { + +// To use tiny solver, create a class or struct that allows computing the cost +// function (described below). This is similar to a ceres::CostFunction, but is +// different to enable statically allocating all memory for the solver +// (specifically, enum sizes). Key parts are the Scalar typedef, the enums to +// describe problem sizes (needed to remove all heap allocations), and the +// operator() overload to evaluate the cost and (optionally) jacobians. +// +// struct TinySolverCostFunctionTraits { +// typedef double Scalar; +// enum { +// NUM_RESIDUALS = <int> OR Eigen::Dynamic, +// NUM_PARAMETERS = <int> OR Eigen::Dynamic, +// }; +// bool operator()(const double* parameters, +// double* residuals, +// double* jacobian) const; +// +// int NumResiduals() const; -- Needed if NUM_RESIDUALS == Eigen::Dynamic. +// int NumParameters() const; -- Needed if NUM_PARAMETERS == Eigen::Dynamic. +// }; +// +// For operator(), the size of the objects is: +// +// double* parameters -- NUM_PARAMETERS or NumParameters() +// double* residuals -- NUM_RESIDUALS or NumResiduals() +// double* jacobian -- NUM_RESIDUALS * NUM_PARAMETERS in column-major format +// (Eigen's default); or NULL if no jacobian requested. +// +// An example (fully statically sized): +// +// struct MyCostFunctionExample { +// typedef double Scalar; +// enum { +// NUM_RESIDUALS = 2, +// NUM_PARAMETERS = 3, +// }; +// bool operator()(const double* parameters, +// double* residuals, +// double* jacobian) const { +// residuals[0] = x + 2*y + 4*z; +// residuals[1] = y * z; +// if (jacobian) { +// jacobian[0 * 2 + 0] = 1; // First column (x). +// jacobian[0 * 2 + 1] = 0; +// +// jacobian[1 * 2 + 0] = 2; // Second column (y). +// jacobian[1 * 2 + 1] = z; +// +// jacobian[2 * 2 + 0] = 4; // Third column (z). +// jacobian[2 * 2 + 1] = y; +// } +// return true; +// } +// }; +// +// The solver supports either statically or dynamically sized cost +// functions. If the number of residuals is dynamic then the Function +// must define: +// +// int NumResiduals() const; +// +// If the number of parameters is dynamic then the Function must +// define: +// +// int NumParameters() const; +// +template <typename Function, + typename LinearSolver = + Eigen::LDLT<Eigen::Matrix<typename Function::Scalar, + Function::NUM_PARAMETERS, + Function::NUM_PARAMETERS>>> +class TinySolver { + public: + // This class needs to have an Eigen aligned operator new as it contains + // fixed-size Eigen types. + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum { + NUM_RESIDUALS = Function::NUM_RESIDUALS, + NUM_PARAMETERS = Function::NUM_PARAMETERS + }; + typedef typename Function::Scalar Scalar; + typedef typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1> Parameters; + + enum Status { + GRADIENT_TOO_SMALL, // eps > max(J'*f(x)) + RELATIVE_STEP_SIZE_TOO_SMALL, // eps > ||dx|| / (||x|| + eps) + COST_TOO_SMALL, // eps > ||f(x)||^2 / 2 + HIT_MAX_ITERATIONS, + + // TODO(sameeragarwal): Deal with numerical failures. + }; + + struct Options { + Scalar gradient_tolerance = 1e-10; // eps > max(J'*f(x)) + Scalar parameter_tolerance = 1e-8; // eps > ||dx|| / ||x|| + Scalar cost_threshold = // eps > ||f(x)|| + std::numeric_limits<Scalar>::epsilon(); + Scalar initial_trust_region_radius = 1e4; + int max_num_iterations = 50; + }; + + struct Summary { + Scalar initial_cost = -1; // 1/2 ||f(x)||^2 + Scalar final_cost = -1; // 1/2 ||f(x)||^2 + Scalar gradient_max_norm = -1; // max(J'f(x)) + int iterations = -1; + Status status = HIT_MAX_ITERATIONS; + }; + + bool Update(const Function& function, const Parameters& x) { + if (!function(x.data(), error_.data(), jacobian_.data())) { + return false; + } + + error_ = -error_; + + // On the first iteration, compute a diagonal (Jacobi) scaling + // matrix, which we store as a vector. + if (summary.iterations == 0) { + // jacobi_scaling = 1 / (1 + diagonal(J'J)) + // + // 1 is added to the denominator to regularize small diagonal + // entries. + jacobi_scaling_ = 1.0 / (1.0 + jacobian_.colwise().norm().array()); + } + + // This explicitly computes the normal equations, which is numerically + // unstable. Nevertheless, it is often good enough and is fast. + // + // TODO(sameeragarwal): Refactor this to allow for DenseQR + // factorization. + jacobian_ = jacobian_ * jacobi_scaling_.asDiagonal(); + jtj_ = jacobian_.transpose() * jacobian_; + g_ = jacobian_.transpose() * error_; + summary.gradient_max_norm = g_.array().abs().maxCoeff(); + cost_ = error_.squaredNorm() / 2; + return true; + } + + const Summary& Solve(const Function& function, Parameters* x_and_min) { + Initialize<NUM_RESIDUALS, NUM_PARAMETERS>(function); + assert(x_and_min); + Parameters& x = *x_and_min; + summary = Summary(); + summary.iterations = 0; + + // TODO(sameeragarwal): Deal with failure here. + Update(function, x); + summary.initial_cost = cost_; + summary.final_cost = cost_; + + if (summary.gradient_max_norm < options.gradient_tolerance) { + summary.status = GRADIENT_TOO_SMALL; + return summary; + } + + if (cost_ < options.cost_threshold) { + summary.status = COST_TOO_SMALL; + return summary; + } + + Scalar u = 1.0 / options.initial_trust_region_radius; + Scalar v = 2; + + for (summary.iterations = 1; + summary.iterations < options.max_num_iterations; + summary.iterations++) { + jtj_regularized_ = jtj_; + const Scalar min_diagonal = 1e-6; + const Scalar max_diagonal = 1e32; + for (int i = 0; i < lm_diagonal_.rows(); ++i) { + lm_diagonal_[i] = std::sqrt( + u * std::min(std::max(jtj_(i, i), min_diagonal), max_diagonal)); + jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i]; + } + + // TODO(sameeragarwal): Check for failure and deal with it. + linear_solver_.compute(jtj_regularized_); + lm_step_ = linear_solver_.solve(g_); + dx_ = jacobi_scaling_.asDiagonal() * lm_step_; + + // Adding parameter_tolerance to x.norm() ensures that this + // works if x is near zero. + const Scalar parameter_tolerance = + options.parameter_tolerance * + (x.norm() + options.parameter_tolerance); + if (dx_.norm() < parameter_tolerance) { + summary.status = RELATIVE_STEP_SIZE_TOO_SMALL; + break; + } + x_new_ = x + dx_; + + // TODO(keir): Add proper handling of errors from user eval of cost + // functions. + function(&x_new_[0], &f_x_new_[0], NULL); + + const Scalar cost_change = (2 * cost_ - f_x_new_.squaredNorm()); + + // TODO(sameeragarwal): Better more numerically stable evaluation. + const Scalar model_cost_change = lm_step_.dot(2 * g_ - jtj_ * lm_step_); + + // rho is the ratio of the actual reduction in error to the reduction + // in error that would be obtained if the problem was linear. See [1] + // for details. + Scalar rho(cost_change / model_cost_change); + if (rho > 0) { + // Accept the Levenberg-Marquardt step because the linear + // model fits well. + x = x_new_; + + // TODO(sameeragarwal): Deal with failure. + Update(function, x); + if (summary.gradient_max_norm < options.gradient_tolerance) { + summary.status = GRADIENT_TOO_SMALL; + break; + } + + if (cost_ < options.cost_threshold) { + summary.status = COST_TOO_SMALL; + break; + } + + Scalar tmp = Scalar(2 * rho - 1); + u = u * std::max(1 / 3., 1 - tmp * tmp * tmp); + v = 2; + continue; + } + + // Reject the update because either the normal equations failed to solve + // or the local linear model was not good (rho < 0). Instead, increase u + // to move closer to gradient descent. + u *= v; + v *= 2; + } + + summary.final_cost = cost_; + return summary; + } + + Options options; + Summary summary; + + private: + // Preallocate everything, including temporary storage needed for solving the + // linear system. This allows reusing the intermediate storage across solves. + LinearSolver linear_solver_; + Scalar cost_; + Parameters dx_, x_new_, g_, jacobi_scaling_, lm_diagonal_, lm_step_; + Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> error_, f_x_new_; + Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_; + Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_; + + // The following definitions are needed for template metaprogramming. + template <bool Condition, typename T> + struct enable_if; + + template <typename T> + struct enable_if<true, T> { + typedef T type; + }; + + // The number of parameters and residuals are dynamically sized. + template <int R, int P> + typename enable_if<(R == Eigen::Dynamic && P == Eigen::Dynamic), void>::type + Initialize(const Function& function) { + Initialize(function.NumResiduals(), function.NumParameters()); + } + + // The number of parameters is dynamically sized and the number of + // residuals is statically sized. + template <int R, int P> + typename enable_if<(R == Eigen::Dynamic && P != Eigen::Dynamic), void>::type + Initialize(const Function& function) { + Initialize(function.NumResiduals(), P); + } + + // The number of parameters is statically sized and the number of + // residuals is dynamically sized. + template <int R, int P> + typename enable_if<(R != Eigen::Dynamic && P == Eigen::Dynamic), void>::type + Initialize(const Function& function) { + Initialize(R, function.NumParameters()); + } + + // The number of parameters and residuals are statically sized. + template <int R, int P> + typename enable_if<(R != Eigen::Dynamic && P != Eigen::Dynamic), void>::type + Initialize(const Function& /* function */) {} + + void Initialize(int num_residuals, int num_parameters) { + dx_.resize(num_parameters); + x_new_.resize(num_parameters); + g_.resize(num_parameters); + jacobi_scaling_.resize(num_parameters); + lm_diagonal_.resize(num_parameters); + lm_step_.resize(num_parameters); + error_.resize(num_residuals); + f_x_new_.resize(num_residuals); + jacobian_.resize(num_residuals, num_parameters); + jtj_.resize(num_parameters, num_parameters); + jtj_regularized_.resize(num_parameters, num_parameters); + } +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_TINY_SOLVER_H_ diff --git a/extern/ceres/include/ceres/tiny_solver_autodiff_function.h b/extern/ceres/include/ceres/tiny_solver_autodiff_function.h new file mode 100644 index 00000000000..b782f549cc1 --- /dev/null +++ b/extern/ceres/include/ceres/tiny_solver_autodiff_function.h @@ -0,0 +1,206 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: mierle@gmail.com (Keir Mierle) +// +// WARNING WARNING WARNING +// WARNING WARNING WARNING Tiny solver is experimental and will change. +// WARNING WARNING WARNING + +#ifndef CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_ +#define CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_ + +#include <memory> +#include <type_traits> + +#include "Eigen/Core" +#include "ceres/jet.h" +#include "ceres/types.h" // For kImpossibleValue. + +namespace ceres { + +// An adapter around autodiff-style CostFunctors to enable easier use of +// TinySolver. See the example below showing how to use it: +// +// // Example for cost functor with static residual size. +// // Same as an autodiff cost functor, but taking only 1 parameter. +// struct MyFunctor { +// template<typename T> +// bool operator()(const T* const parameters, T* residuals) const { +// const T& x = parameters[0]; +// const T& y = parameters[1]; +// const T& z = parameters[2]; +// residuals[0] = x + 2.*y + 4.*z; +// residuals[1] = y * z; +// return true; +// } +// }; +// +// typedef TinySolverAutoDiffFunction<MyFunctor, 2, 3> +// AutoDiffFunction; +// +// MyFunctor my_functor; +// AutoDiffFunction f(my_functor); +// +// Vec3 x = ...; +// TinySolver<AutoDiffFunction> solver; +// solver.Solve(f, &x); +// +// // Example for cost functor with dynamic residual size. +// // NumResiduals() supplies dynamic size of residuals. +// // Same functionality as in tiny_solver.h but with autodiff. +// struct MyFunctorWithDynamicResiduals { +// int NumResiduals() const { +// return 2; +// } +// +// template<typename T> +// bool operator()(const T* const parameters, T* residuals) const { +// const T& x = parameters[0]; +// const T& y = parameters[1]; +// const T& z = parameters[2]; +// residuals[0] = x + static_cast<T>(2.)*y + static_cast<T>(4.)*z; +// residuals[1] = y * z; +// return true; +// } +// }; +// +// typedef TinySolverAutoDiffFunction<MyFunctorWithDynamicResiduals, +// Eigen::Dynamic, +// 3> +// AutoDiffFunctionWithDynamicResiduals; +// +// MyFunctorWithDynamicResiduals my_functor_dyn; +// AutoDiffFunctionWithDynamicResiduals f(my_functor_dyn); +// +// Vec3 x = ...; +// TinySolver<AutoDiffFunctionWithDynamicResiduals> solver; +// solver.Solve(f, &x); +// +// WARNING: The cost function adapter is not thread safe. +template <typename CostFunctor, + int kNumResiduals, + int kNumParameters, + typename T = double> +class TinySolverAutoDiffFunction { + public: + // This class needs to have an Eigen aligned operator new as it contains + // as a member a Jet type, which itself has a fixed-size Eigen type as member. + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + TinySolverAutoDiffFunction(const CostFunctor& cost_functor) + : cost_functor_(cost_functor) { + Initialize<kNumResiduals>(cost_functor); + } + + typedef T Scalar; + enum { + NUM_PARAMETERS = kNumParameters, + NUM_RESIDUALS = kNumResiduals, + }; + + // This is similar to AutoDifferentiate(), but since there is only one + // parameter block it is easier to inline to avoid overhead. + bool operator()(const T* parameters, T* residuals, T* jacobian) const { + if (jacobian == NULL) { + // No jacobian requested, so just directly call the cost function with + // doubles, skipping jets and derivatives. + return cost_functor_(parameters, residuals); + } + // Initialize the input jets with passed parameters. + for (int i = 0; i < kNumParameters; ++i) { + jet_parameters_[i].a = parameters[i]; // Scalar part. + jet_parameters_[i].v.setZero(); // Derivative part. + jet_parameters_[i].v[i] = T(1.0); + } + + // Initialize the output jets such that we can detect user errors. + for (int i = 0; i < num_residuals_; ++i) { + jet_residuals_[i].a = kImpossibleValue; + jet_residuals_[i].v.setConstant(kImpossibleValue); + } + + // Execute the cost function, but with jets to find the derivative. + if (!cost_functor_(jet_parameters_, jet_residuals_.data())) { + return false; + } + + // Copy the jacobian out of the derivative part of the residual jets. + Eigen::Map<Eigen::Matrix<T, kNumResiduals, kNumParameters>> jacobian_matrix( + jacobian, num_residuals_, kNumParameters); + for (int r = 0; r < num_residuals_; ++r) { + residuals[r] = jet_residuals_[r].a; + // Note that while this looks like a fast vectorized write, in practice it + // unfortunately thrashes the cache since the writes to the column-major + // jacobian are strided (e.g. rows are non-contiguous). + jacobian_matrix.row(r) = jet_residuals_[r].v; + } + return true; + } + + int NumResiduals() const { + return num_residuals_; // Set by Initialize. + } + + private: + const CostFunctor& cost_functor_; + + // The number of residuals at runtime. + // This will be overriden if NUM_RESIDUALS == Eigen::Dynamic. + int num_residuals_ = kNumResiduals; + + // To evaluate the cost function with jets, temporary storage is needed. These + // are the buffers that are used during evaluation; parameters for the input, + // and jet_residuals_ are where the final cost and derivatives end up. + // + // Since this buffer is used for evaluation, the adapter is not thread safe. + using JetType = Jet<T, kNumParameters>; + mutable JetType jet_parameters_[kNumParameters]; + // Eigen::Matrix serves as static or dynamic container. + mutable Eigen::Matrix<JetType, kNumResiduals, 1> jet_residuals_; + + // The number of residuals is dynamically sized and the number of + // parameters is statically sized. + template <int R> + typename std::enable_if<(R == Eigen::Dynamic), void>::type Initialize( + const CostFunctor& function) { + jet_residuals_.resize(function.NumResiduals()); + num_residuals_ = function.NumResiduals(); + } + + // The number of parameters and residuals are statically sized. + template <int R> + typename std::enable_if<(R != Eigen::Dynamic), void>::type Initialize( + const CostFunctor& /* function */) { + num_residuals_ = kNumResiduals; + } +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/tiny_solver_cost_function_adapter.h b/extern/ceres/include/ceres/tiny_solver_cost_function_adapter.h new file mode 100644 index 00000000000..18ccb398f90 --- /dev/null +++ b/extern/ceres/include/ceres/tiny_solver_cost_function_adapter.h @@ -0,0 +1,142 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_TINY_SOLVER_COST_FUNCTION_ADAPTER_H_ +#define CERES_PUBLIC_TINY_SOLVER_COST_FUNCTION_ADAPTER_H_ + +#include "Eigen/Core" +#include "ceres/cost_function.h" +#include "glog/logging.h" + +namespace ceres { + +// An adapter class that lets users of TinySolver use +// ceres::CostFunction objects that have exactly one parameter block. +// +// The adapter allows for the number of residuals and the size of the +// parameter block to be specified at compile or run-time. +// +// WARNING: This object is not thread-safe. +// +// Example usage: +// +// CostFunction* cost_function = ... +// +// Number of residuals and parameter block size known at compile time: +// +// TinySolverCostFunctionAdapter<kNumResiduals, kNumParameters> +// cost_function_adapter(*cost_function); +// +// Number of residuals known at compile time and the parameter block +// size not known at compile time. +// +// TinySolverCostFunctionAdapter<kNumResiduals, Eigen::Dynamic> +// cost_function_adapter(*cost_function); +// +// Number of residuals not known at compile time and the parameter +// block size known at compile time. +// +// TinySolverCostFunctionAdapter<Eigen::Dynamic, kParameterBlockSize> +// cost_function_adapter(*cost_function); +// +// Number of residuals not known at compile time and the parameter +// block size not known at compile time. +// +// TinySolverCostFunctionAdapter cost_function_adapter(*cost_function); +// +template <int kNumResiduals = Eigen::Dynamic, + int kNumParameters = Eigen::Dynamic> +class TinySolverCostFunctionAdapter { + public: + typedef double Scalar; + enum ComponentSizeType { + NUM_PARAMETERS = kNumParameters, + NUM_RESIDUALS = kNumResiduals + }; + + // This struct needs to have an Eigen aligned operator new as it contains + // fixed-size Eigen types. + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + TinySolverCostFunctionAdapter(const CostFunction& cost_function) + : cost_function_(cost_function) { + CHECK_EQ(cost_function_.parameter_block_sizes().size(), 1) + << "Only CostFunctions with exactly one parameter blocks are allowed."; + + const int parameter_block_size = cost_function_.parameter_block_sizes()[0]; + if (NUM_PARAMETERS == Eigen::Dynamic || NUM_RESIDUALS == Eigen::Dynamic) { + if (NUM_RESIDUALS != Eigen::Dynamic) { + CHECK_EQ(cost_function_.num_residuals(), NUM_RESIDUALS); + } + if (NUM_PARAMETERS != Eigen::Dynamic) { + CHECK_EQ(parameter_block_size, NUM_PARAMETERS); + } + + row_major_jacobian_.resize(cost_function_.num_residuals(), + parameter_block_size); + } + } + + bool operator()(const double* parameters, + double* residuals, + double* jacobian) const { + if (!jacobian) { + return cost_function_.Evaluate(¶meters, residuals, NULL); + } + + double* jacobians[1] = {row_major_jacobian_.data()}; + if (!cost_function_.Evaluate(¶meters, residuals, jacobians)) { + return false; + } + + // The Function object used by TinySolver takes its Jacobian in a + // column-major layout, and the CostFunction objects use row-major + // Jacobian matrices. So the following bit of code does the + // conversion from row-major Jacobians to column-major Jacobians. + Eigen::Map<Eigen::Matrix<double, NUM_RESIDUALS, NUM_PARAMETERS>> + col_major_jacobian(jacobian, NumResiduals(), NumParameters()); + col_major_jacobian = row_major_jacobian_; + return true; + } + + int NumResiduals() const { return cost_function_.num_residuals(); } + int NumParameters() const { + return cost_function_.parameter_block_sizes()[0]; + } + + private: + const CostFunction& cost_function_; + mutable Eigen::Matrix<double, NUM_RESIDUALS, NUM_PARAMETERS, Eigen::RowMajor> + row_major_jacobian_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_TINY_SOLVER_COST_FUNCTION_ADAPTER_H_ diff --git a/extern/ceres/include/ceres/types.h b/extern/ceres/include/ceres/types.h index 2ea41803629..3a19b7333b2 100644 --- a/extern/ceres/include/ceres/types.h +++ b/extern/ceres/include/ceres/types.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -39,15 +39,11 @@ #include <string> -#include "ceres/internal/port.h" #include "ceres/internal/disable_warnings.h" +#include "ceres/internal/port.h" namespace ceres { -// Basic integer types. These typedefs are in the Ceres namespace to avoid -// conflicts with other packages having similar typedefs. -typedef int int32; - // Argument type used in interfaces that can optionally take ownership // of a passed in argument. If TAKE_OWNERSHIP is passed, the called // object takes ownership of the pointer argument, and will call @@ -116,10 +112,29 @@ enum PreconditionerType { // the scene to determine the sparsity structure of the // preconditioner. This is done using a clustering algorithm. The // available visibility clustering algorithms are described below. - // - // Note: Requires SuiteSparse. CLUSTER_JACOBI, - CLUSTER_TRIDIAGONAL + CLUSTER_TRIDIAGONAL, + + // Subset preconditioner is a general purpose preconditioner + // linear least squares problems. Given a set of residual blocks, + // it uses the corresponding subset of the rows of the Jacobian to + // construct a preconditioner. + // + // Suppose the Jacobian J has been horizontally partitioned as + // + // J = [P] + // [Q] + // + // Where, Q is the set of rows corresponding to the residual + // blocks in residual_blocks_for_subset_preconditioner. + // + // The preconditioner is the inverse of the matrix Q'Q. + // + // Obviously, the efficacy of the preconditioner depends on how + // well the matrix Q approximates J'J, or how well the chosen + // residual blocks approximate the non-linear least squares + // problem. + SUBSET, }; enum VisibilityClusteringType { @@ -150,7 +165,7 @@ enum SparseLinearAlgebraLibraryType { // minimum degree ordering. SUITE_SPARSE, - // A lightweight replacment for SuiteSparse, which does not require + // A lightweight replacement for SuiteSparse, which does not require // a LAPACK/BLAS implementation. Consequently, its performance is // also a bit lower than SuiteSparse. CX_SPARSE, @@ -159,6 +174,9 @@ enum SparseLinearAlgebraLibraryType { // the Simplicial LDLT routines. EIGEN_SPARSE, + // Apple's Accelerate framework sparse linear algebra routines. + ACCELERATE_SPARSE, + // No sparse linear solver should be used. This does not necessarily // imply that Ceres was built without any sparse library, although that // is the likely use case, merely that one should not be used. @@ -202,7 +220,7 @@ enum LineSearchDirectionType { // symmetric matrix but only N conditions are specified by the Secant // equation. The requirement that the Hessian approximation be positive // definite imposes another N additional constraints, but that still leaves - // remaining degrees-of-freedom. (L)BFGS methods uniquely deteremine the + // remaining degrees-of-freedom. (L)BFGS methods uniquely determine the // approximate Hessian by imposing the additional constraints that the // approximation at the next iteration must be the 'closest' to the current // approximation (the nature of how this proximity is measured is actually @@ -222,26 +240,26 @@ enum LineSearchDirectionType { // For more details on BFGS see: // // Broyden, C.G., "The Convergence of a Class of Double-rank Minimization - // Algorithms,"; J. Inst. Maths. Applics., Vol. 6, pp 76–90, 1970. + // Algorithms,"; J. Inst. Maths. Applics., Vol. 6, pp 76-90, 1970. // // Fletcher, R., "A New Approach to Variable Metric Algorithms," - // Computer Journal, Vol. 13, pp 317–322, 1970. + // Computer Journal, Vol. 13, pp 317-322, 1970. // // Goldfarb, D., "A Family of Variable Metric Updates Derived by Variational - // Means," Mathematics of Computing, Vol. 24, pp 23–26, 1970. + // Means," Mathematics of Computing, Vol. 24, pp 23-26, 1970. // // Shanno, D.F., "Conditioning of Quasi-Newton Methods for Function - // Minimization," Mathematics of Computing, Vol. 24, pp 647–656, 1970. + // Minimization," Mathematics of Computing, Vol. 24, pp 647-656, 1970. // // For more details on L-BFGS see: // // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with Limited - // Storage". Mathematics of Computation 35 (151): 773–782. + // Storage". Mathematics of Computation 35 (151): 773-782. // // Byrd, R. H.; Nocedal, J.; Schnabel, R. B. (1994). // "Representations of Quasi-Newton Matrices and their use in // Limited Memory Methods". Mathematical Programming 63 (4): - // 129–156. + // 129-156. // // A general reference for both methods: // @@ -250,7 +268,7 @@ enum LineSearchDirectionType { BFGS, }; -// Nonliner conjugate gradient methods are a generalization of the +// Nonlinear conjugate gradient methods are a generalization of the // method of Conjugate Gradients for linear systems. The // generalization can be carried out in a number of different ways // leading to number of different rules for computing the search @@ -420,10 +438,16 @@ enum LineSearchInterpolationType { enum CovarianceAlgorithmType { DENSE_SVD, - SUITE_SPARSE_QR, - EIGEN_SPARSE_QR + SPARSE_QR, }; +// It is a near impossibility that user code generates this exact +// value in normal operation, thus we will use it to fill arrays +// before passing them to user code. If on return an element of the +// array still contains this value, we will assume that the user code +// did not write to that memory location. +const double kImpossibleValue = 1e302; + CERES_EXPORT const char* LinearSolverTypeToString( LinearSolverType type); CERES_EXPORT bool StringToLinearSolverType(std::string value, @@ -493,6 +517,13 @@ CERES_EXPORT bool StringToNumericDiffMethodType( std::string value, NumericDiffMethodType* type); +CERES_EXPORT const char* LoggingTypeToString(LoggingType type); +CERES_EXPORT bool StringtoLoggingType(std::string value, LoggingType* type); + +CERES_EXPORT const char* DumpFormatTypeToString(DumpFormatType type); +CERES_EXPORT bool StringtoDumpFormatType(std::string value, DumpFormatType* type); +CERES_EXPORT bool StringtoDumpFormatType(std::string value, LoggingType* type); + CERES_EXPORT const char* TerminationTypeToString(TerminationType type); CERES_EXPORT bool IsSchurType(LinearSolverType type); diff --git a/extern/ceres/include/ceres/version.h b/extern/ceres/include/ceres/version.h index 2f1cc297a38..50aa2124e75 100644 --- a/extern/ceres/include/ceres/version.h +++ b/extern/ceres/include/ceres/version.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,8 +31,8 @@ #ifndef CERES_PUBLIC_VERSION_H_ #define CERES_PUBLIC_VERSION_H_ -#define CERES_VERSION_MAJOR 1 -#define CERES_VERSION_MINOR 12 +#define CERES_VERSION_MAJOR 2 +#define CERES_VERSION_MINOR 0 #define CERES_VERSION_REVISION 0 // Classic CPP stringifcation; the extra level of indirection allows the |