Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h
new file mode 100644
index 0000000..8851bc7
--- /dev/null
+++ b/include/ceres/internal/numeric_diff.h
@@ -0,0 +1,499 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+// mierle@gmail.com (Keir Mierle)
+// tbennun@gmail.com (Tal Ben-Nun)
+//
+// Finite differencing routines used by NumericDiffCostFunction.
+
+#ifndef CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_
+#define CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_
+
+#include <cstring>
+
+#include "Eigen/Dense"
+#include "Eigen/StdVector"
+#include "ceres/cost_function.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/internal/variadic_evaluate.h"
+#include "ceres/numeric_diff_options.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+
+namespace ceres {
+namespace internal {
+
+// 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, typename ParameterDims, int kParameterBlock,
+ int kParameterBlockSize>
+struct NumericDiff {
+ // Mutates parameters but must restore them before return.
+ static bool EvaluateJacobianForParameterBlock(
+ const CostFunctor* functor,
+ const double* residuals_at_eval_point,
+ const NumericDiffOptions& options,
+ int num_residuals,
+ int parameter_block_index,
+ int parameter_block_size,
+ double **parameters,
+ double *jacobian) {
+ using Eigen::Map;
+ using Eigen::Matrix;
+ using Eigen::RowMajor;
+ using Eigen::ColMajor;
+
+ DCHECK(jacobian);
+
+ const int num_residuals_internal =
+ (kNumResiduals != ceres::DYNAMIC ? kNumResiduals : num_residuals);
+ const int parameter_block_index_internal =
+ (kParameterBlock != ceres::DYNAMIC ? kParameterBlock :
+ parameter_block_index);
+ const int parameter_block_size_internal =
+ (kParameterBlockSize != ceres::DYNAMIC ? kParameterBlockSize :
+ parameter_block_size);
+
+ typedef Matrix<double, kNumResiduals, 1> ResidualVector;
+ typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
+
+ // The convoluted reasoning for choosing the Row/Column major
+ // ordering of the matrix is an artifact of the restrictions in
+ // Eigen that prevent it from creating RowMajor matrices with a
+ // single column. In these cases, we ask for a ColMajor matrix.
+ typedef Matrix<double,
+ kNumResiduals,
+ kParameterBlockSize,
+ (kParameterBlockSize == 1) ? ColMajor : RowMajor>
+ JacobianMatrix;
+
+ Map<JacobianMatrix> parameter_jacobian(jacobian,
+ num_residuals_internal,
+ parameter_block_size_internal);
+
+ Map<ParameterVector> x_plus_delta(
+ parameters[parameter_block_index_internal],
+ parameter_block_size_internal);
+ ParameterVector x(x_plus_delta);
+ ParameterVector step_size = x.array().abs() *
+ ((kMethod == RIDDERS) ? options.ridders_relative_initial_step_size :
+ options.relative_step_size);
+
+ // It is not a good idea to make the step size arbitrarily
+ // small. This will lead to problems with round off and numerical
+ // instability when dividing by the step size. The general
+ // recommendation is to not go down below sqrt(epsilon).
+ double min_step_size = std::sqrt(std::numeric_limits<double>::epsilon());
+
+ // For Ridders' method, the initial step size is required to be large,
+ // thus ridders_relative_initial_step_size is used.
+ if (kMethod == RIDDERS) {
+ min_step_size = std::max(min_step_size,
+ options.ridders_relative_initial_step_size);
+ }
+
+ // For each parameter in the parameter block, use finite differences to
+ // compute the derivative for that parameter.
+ FixedArray<double> temp_residual_array(num_residuals_internal);
+ FixedArray<double> residual_array(num_residuals_internal);
+ Map<ResidualVector> residuals(residual_array.get(),
+ num_residuals_internal);
+
+ for (int j = 0; j < parameter_block_size_internal; ++j) {
+ const double delta = std::max(min_step_size, step_size(j));
+
+ if (kMethod == RIDDERS) {
+ if (!EvaluateRiddersJacobianColumn(functor, j, delta,
+ options,
+ num_residuals_internal,
+ parameter_block_size_internal,
+ x.data(),
+ residuals_at_eval_point,
+ parameters,
+ x_plus_delta.data(),
+ temp_residual_array.get(),
+ residual_array.get())) {
+ return false;
+ }
+ } else {
+ if (!EvaluateJacobianColumn(functor, j, delta,
+ num_residuals_internal,
+ parameter_block_size_internal,
+ x.data(),
+ residuals_at_eval_point,
+ parameters,
+ x_plus_delta.data(),
+ temp_residual_array.get(),
+ residual_array.get())) {
+ return false;
+ }
+ }
+
+ parameter_jacobian.col(j).matrix() = residuals;
+ }
+ return true;
+ }
+
+ static bool EvaluateJacobianColumn(const CostFunctor* functor,
+ int parameter_index,
+ double delta,
+ int num_residuals,
+ int parameter_block_size,
+ const double* x_ptr,
+ const double* residuals_at_eval_point,
+ double** parameters,
+ double* x_plus_delta_ptr,
+ double* temp_residuals_ptr,
+ double* residuals_ptr) {
+ using Eigen::Map;
+ using Eigen::Matrix;
+
+ typedef Matrix<double, kNumResiduals, 1> ResidualVector;
+ typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
+
+ Map<const ParameterVector> x(x_ptr, parameter_block_size);
+ Map<ParameterVector> x_plus_delta(x_plus_delta_ptr,
+ parameter_block_size);
+
+ Map<ResidualVector> residuals(residuals_ptr, num_residuals);
+ Map<ResidualVector> temp_residuals(temp_residuals_ptr, num_residuals);
+
+ // Mutate 1 element at a time and then restore.
+ x_plus_delta(parameter_index) = x(parameter_index) + delta;
+
+ if (!VariadicEvaluate<ParameterDims>(*functor,
+ parameters,
+ residuals.data())) {
+ return false;
+ }
+
+ // Compute this column of the jacobian in 3 steps:
+ // 1. Store residuals for the forward part.
+ // 2. Subtract residuals for the backward (or 0) part.
+ // 3. Divide out the run.
+ double one_over_delta = 1.0 / delta;
+ if (kMethod == CENTRAL || kMethod == RIDDERS) {
+ // Compute the function on the other side of x(parameter_index).
+ x_plus_delta(parameter_index) = x(parameter_index) - delta;
+
+ if (!VariadicEvaluate<ParameterDims>(*functor,
+ parameters,
+ temp_residuals.data())) {
+ return false;
+ }
+
+ residuals -= temp_residuals;
+ one_over_delta /= 2;
+ } else {
+ // Forward difference only; reuse existing residuals evaluation.
+ residuals -=
+ Map<const ResidualVector>(residuals_at_eval_point,
+ num_residuals);
+ }
+
+ // Restore x_plus_delta.
+ x_plus_delta(parameter_index) = x(parameter_index);
+
+ // Divide out the run to get slope.
+ residuals *= one_over_delta;
+
+ return true;
+ }
+
+ // This numeric difference implementation uses adaptive differentiation
+ // on the parameters to obtain the Jacobian matrix. The adaptive algorithm
+ // is based on Ridders' method for adaptive differentiation, which creates
+ // a Romberg tableau from varying step sizes and extrapolates the
+ // intermediate results to obtain the current computational error.
+ //
+ // References:
+ // C.J.F. Ridders, Accurate computation of F'(x) and F'(x) F"(x), Advances
+ // in Engineering Software (1978), Volume 4, Issue 2, April 1982,
+ // Pages 75-76, ISSN 0141-1195,
+ // http://dx.doi.org/10.1016/S0141-1195(82)80057-0.
+ static bool EvaluateRiddersJacobianColumn(
+ const CostFunctor* functor,
+ int parameter_index,
+ double delta,
+ const NumericDiffOptions& options,
+ int num_residuals,
+ int parameter_block_size,
+ const double* x_ptr,
+ const double* residuals_at_eval_point,
+ double** parameters,
+ double* x_plus_delta_ptr,
+ double* temp_residuals_ptr,
+ double* residuals_ptr) {
+ using Eigen::Map;
+ using Eigen::Matrix;
+ using Eigen::aligned_allocator;
+
+ typedef Matrix<double, kNumResiduals, 1> ResidualVector;
+ typedef Matrix<double, kNumResiduals, Eigen::Dynamic> ResidualCandidateMatrix;
+ typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
+
+ Map<const ParameterVector> x(x_ptr, parameter_block_size);
+ Map<ParameterVector> x_plus_delta(x_plus_delta_ptr,
+ parameter_block_size);
+
+ Map<ResidualVector> residuals(residuals_ptr, num_residuals);
+ Map<ResidualVector> temp_residuals(temp_residuals_ptr, num_residuals);
+
+ // In order for the algorithm to converge, the step size should be
+ // initialized to a value that is large enough to produce a significant
+ // change in the function.
+ // As the derivative is estimated, the step size decreases.
+ // By default, the step sizes are chosen so that the middle column
+ // of the Romberg tableau uses the input delta.
+ double current_step_size = delta *
+ pow(options.ridders_step_shrink_factor,
+ options.max_num_ridders_extrapolations / 2);
+
+ // Double-buffering temporary differential candidate vectors
+ // from previous step size.
+ ResidualCandidateMatrix stepsize_candidates_a(
+ num_residuals,
+ options.max_num_ridders_extrapolations);
+ ResidualCandidateMatrix stepsize_candidates_b(
+ num_residuals,
+ options.max_num_ridders_extrapolations);
+ ResidualCandidateMatrix* current_candidates = &stepsize_candidates_a;
+ ResidualCandidateMatrix* previous_candidates = &stepsize_candidates_b;
+
+ // Represents the computational error of the derivative. This variable is
+ // initially set to a large value, and is set to the difference between
+ // current and previous finite difference extrapolations.
+ // norm_error is supposed to decrease as the finite difference tableau
+ // generation progresses, serving both as an estimate for differentiation
+ // error and as a measure of differentiation numerical stability.
+ double norm_error = std::numeric_limits<double>::max();
+
+ // Loop over decreasing step sizes until:
+ // 1. Error is smaller than a given value (ridders_epsilon),
+ // 2. Maximal order of extrapolation reached, or
+ // 3. Extrapolation becomes numerically unstable.
+ for (int i = 0; i < options.max_num_ridders_extrapolations; ++i) {
+ // Compute the numerical derivative at this step size.
+ if (!EvaluateJacobianColumn(functor, parameter_index, current_step_size,
+ num_residuals,
+ parameter_block_size,
+ x.data(),
+ residuals_at_eval_point,
+ parameters,
+ x_plus_delta.data(),
+ temp_residuals.data(),
+ current_candidates->col(0).data())) {
+ // Something went wrong; bail.
+ return false;
+ }
+
+ // Store initial results.
+ if (i == 0) {
+ residuals = current_candidates->col(0);
+ }
+
+ // Shrink differentiation step size.
+ current_step_size /= options.ridders_step_shrink_factor;
+
+ // Extrapolation factor for Richardson acceleration method (see below).
+ double richardson_factor = options.ridders_step_shrink_factor *
+ options.ridders_step_shrink_factor;
+ for (int k = 1; k <= i; ++k) {
+ // Extrapolate the various orders of finite differences using
+ // the Richardson acceleration method.
+ current_candidates->col(k) =
+ (richardson_factor * current_candidates->col(k - 1) -
+ previous_candidates->col(k - 1)) / (richardson_factor - 1.0);
+
+ richardson_factor *= options.ridders_step_shrink_factor *
+ options.ridders_step_shrink_factor;
+
+ // Compute the difference between the previous value and the current.
+ double candidate_error = std::max(
+ (current_candidates->col(k) -
+ current_candidates->col(k - 1)).norm(),
+ (current_candidates->col(k) -
+ previous_candidates->col(k - 1)).norm());
+
+ // If the error has decreased, update results.
+ if (candidate_error <= norm_error) {
+ norm_error = candidate_error;
+ residuals = current_candidates->col(k);
+
+ // If the error is small enough, stop.
+ if (norm_error < options.ridders_epsilon) {
+ break;
+ }
+ }
+ }
+
+ // After breaking out of the inner loop, declare convergence.
+ if (norm_error < options.ridders_epsilon) {
+ break;
+ }
+
+ // Check to see if the current gradient estimate is numerically unstable.
+ // If so, bail out and return the last stable result.
+ if (i > 0) {
+ double tableau_error = (current_candidates->col(i) -
+ previous_candidates->col(i - 1)).norm();
+
+ // Compare current error to the chosen candidate's error.
+ if (tableau_error >= 2 * norm_error) {
+ break;
+ }
+ }
+
+ std::swap(current_candidates, previous_candidates);
+ }
+ return true;
+ }
+};
+
+// 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,
+ 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,
+ 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, 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;
+ }
+};
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_