diff options
Diffstat (limited to 'internal/ceres/runtime_numeric_diff_cost_function.cc')
| -rw-r--r-- | internal/ceres/runtime_numeric_diff_cost_function.cc | 217 |
1 files changed, 0 insertions, 217 deletions
diff --git a/internal/ceres/runtime_numeric_diff_cost_function.cc b/internal/ceres/runtime_numeric_diff_cost_function.cc deleted file mode 100644 index 7af275c..0000000 --- a/internal/ceres/runtime_numeric_diff_cost_function.cc +++ /dev/null @@ -1,217 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) -// -// Based on the templated version in public/numeric_diff_cost_function.h. - -#include "ceres/runtime_numeric_diff_cost_function.h" - -#include <algorithm> -#include <numeric> -#include <vector> -#include "Eigen/Dense" -#include "ceres/cost_function.h" -#include "ceres/internal/scoped_ptr.h" -#include "glog/logging.h" - -namespace ceres { -namespace internal { -namespace { - -bool EvaluateJacobianForParameterBlock(const CostFunction* function, - int parameter_block_size, - int parameter_block, - RuntimeNumericDiffMethod method, - double relative_step_size, - double const* residuals_at_eval_point, - double** parameters, - double** jacobians) { - using Eigen::Map; - using Eigen::Matrix; - using Eigen::Dynamic; - using Eigen::RowMajor; - - typedef Matrix<double, Dynamic, 1> ResidualVector; - typedef Matrix<double, Dynamic, 1> ParameterVector; - typedef Matrix<double, Dynamic, Dynamic, RowMajor> JacobianMatrix; - - int num_residuals = function->num_residuals(); - - Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block], - num_residuals, - parameter_block_size); - - // Mutate one element at a time and then restore. - Map<ParameterVector> x_plus_delta(parameters[parameter_block], - parameter_block_size); - ParameterVector x(x_plus_delta); - ParameterVector step_size = x.array().abs() * relative_step_size; - - // To handle cases where a paremeter is exactly zero, instead use the mean - // step_size for the other dimensions. - double fallback_step_size = step_size.sum() / step_size.rows(); - if (fallback_step_size == 0.0) { - // If all the parameters are zero, there's no good answer. Use the given - // relative step_size as absolute step_size and hope for the best. - fallback_step_size = relative_step_size; - } - - // For each parameter in the parameter block, use finite differences to - // compute the derivative for that parameter. - for (int j = 0; j < parameter_block_size; ++j) { - if (step_size(j) == 0.0) { - // The parameter is exactly zero, so compromise and use the mean step_size - // from the other parameters. This can break in many cases, but it's hard - // to pick a good number without problem specific knowledge. - step_size(j) = fallback_step_size; - } - x_plus_delta(j) = x(j) + step_size(j); - - ResidualVector residuals(num_residuals); - if (!function->Evaluate(parameters, &residuals[0], NULL)) { - // Something went wrong; bail. - 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. - parameter_jacobian.col(j) = residuals; - - double one_over_h = 1 / step_size(j); - if (method == CENTRAL) { - // Compute the function on the other side of x(j). - x_plus_delta(j) = x(j) - step_size(j); - - if (!function->Evaluate(parameters, &residuals[0], NULL)) { - // Something went wrong; bail. - return false; - } - parameter_jacobian.col(j) -= residuals; - one_over_h /= 2; - } else { - // Forward difference only; reuse existing residuals evaluation. - parameter_jacobian.col(j) -= - Map<const ResidualVector>(residuals_at_eval_point, num_residuals); - } - x_plus_delta(j) = x(j); // Restore x_plus_delta. - - // Divide out the run to get slope. - parameter_jacobian.col(j) *= one_over_h; - } - return true; -} - -class RuntimeNumericDiffCostFunction : public CostFunction { - public: - RuntimeNumericDiffCostFunction(const CostFunction* function, - RuntimeNumericDiffMethod method, - double relative_step_size) - : function_(function), - method_(method), - relative_step_size_(relative_step_size) { - *mutable_parameter_block_sizes() = function->parameter_block_sizes(); - set_num_residuals(function->num_residuals()); - } - - virtual ~RuntimeNumericDiffCostFunction() { } - - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { - // Get the function value (residuals) at the the point to evaluate. - bool success = function_->Evaluate(parameters, residuals, NULL); - if (!success) { - // Something went wrong; ignore the jacobian. - return false; - } - if (!jacobians) { - // Nothing to do; just forward. - return true; - } - - const vector<int16>& block_sizes = function_->parameter_block_sizes(); - CHECK(!block_sizes.empty()); - - // Create local space for a copy of the parameters which will get mutated. - int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0); - vector<double> parameters_copy(parameters_size); - vector<double*> parameters_references_copy(block_sizes.size()); - parameters_references_copy[0] = ¶meters_copy[0]; - for (int block = 1; block < block_sizes.size(); ++block) { - parameters_references_copy[block] = parameters_references_copy[block - 1] - + block_sizes[block - 1]; - } - - // Copy the parameters into the local temp space. - for (int block = 0; block < block_sizes.size(); ++block) { - memcpy(parameters_references_copy[block], - parameters[block], - block_sizes[block] * sizeof(*parameters[block])); - } - - for (int block = 0; block < block_sizes.size(); ++block) { - if (!jacobians[block]) { - // No jacobian requested for this parameter / residual pair. - continue; - } - if (!EvaluateJacobianForParameterBlock(function_, - block_sizes[block], - block, - method_, - relative_step_size_, - residuals, - ¶meters_references_copy[0], - jacobians)) { - return false; - } - } - return true; - } - - private: - const CostFunction* function_; - RuntimeNumericDiffMethod method_; - double relative_step_size_; -}; - -} // namespace - -CostFunction* CreateRuntimeNumericDiffCostFunction( - const CostFunction* cost_function, - RuntimeNumericDiffMethod method, - double relative_step_size) { - return new RuntimeNumericDiffCostFunction(cost_function, - method, - relative_step_size); -} - -} // namespace internal -} // namespace ceres |