internal/ceres/runtime_numeric_diff_cost_function.cc - ceres-solver - Git at Google

 // 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] = &parameters_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,
                                              &parameters_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
	// 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] = &parameters_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,
	&parameters_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