include/ceres/numeric_diff_cost_function.h - 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)
 //
 // Create CostFunctions as needed by the least squares framework with jacobians
 // computed via numeric (a.k.a. finite) differentiation. For more details see
 // http://en.wikipedia.org/wiki/Numerical_differentiation.
 //
 // To get a numerically differentiated cost function, define a subclass of
 // CostFunction such that the Evaluate() function ignores the jacobian
 // parameter. The numeric differentiation wrapper will fill in the jacobian
 // parameter if nececssary by repeatedly calling the Evaluate() function with
 // small changes to the appropriate parameters, and computing the slope. For
 // performance, the numeric differentiation wrapper class is templated on the
 // concrete cost function, even though it could be implemented only in terms of
 // the virtual CostFunction interface.
 //
 // The numerically differentiated version of a cost function for a cost function
 // can be constructed as follows:
 //
 //   CostFunction* cost_function
 //       = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>(
 //           new MyCostFunction(...), TAKE_OWNERSHIP);
 //
 // where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8
 // respectively. Look at the tests for a more detailed example.
 //
 // The central difference method is considerably more accurate at the cost of
 // twice as many function evaluations than forward difference. Consider using
 // central differences begin with, and only after that works, trying forward
 // difference to improve performance.
 //
 // TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives.

 #ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
 #define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_

 #include <cstring>
 #include <glog/logging.h>
 #include "Eigen/Dense"
 #include "ceres/internal/scoped_ptr.h"
 #include "ceres/sized_cost_function.h"
 #include "ceres/types.h"

 namespace ceres {

 enum NumericDiffMethod {
   CENTRAL,
   FORWARD
 };

 // 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 CostFunctionNoJacobian,
           int num_residuals,
           int parameter_block_size,
           int parameter_block,
           NumericDiffMethod method>
 struct Differencer {
   // Mutates parameters but must restore them before return.
   static bool EvaluateJacobianForParameterBlock(
       const CostFunctionNoJacobian *function,
       double const* residuals_at_eval_point,
       double **parameters,
       double **jacobians) {
     using Eigen::Map;
     using Eigen::Matrix;
     using Eigen::RowMajor;
     using Eigen::ColMajor;

     typedef Matrix<double, num_residuals, 1> ResidualVector;
     typedef Matrix<double, parameter_block_size, 1> ParameterVector;
     typedef Matrix<double, num_residuals, parameter_block_size,
                    (parameter_block_size == 1 &&
                     num_residuals > 1) ? ColMajor : RowMajor> JacobianMatrix;

     Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block],
                                            num_residuals,
                                            parameter_block_size);

     // Mutate 1 element at a time and then restore.
     Map<ParameterVector> x_plus_delta(parameters[parameter_block],
                                       parameter_block_size);
     ParameterVector x(x_plus_delta);

     // TODO(keir): Pick a smarter number! In 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.
     const double kRelativeStepSize = 1e-6;

     ParameterVector step_size = x.array().abs() * kRelativeStepSize;

     // To handle cases where a parameter 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. Take
       // kRelativeStepSize as a guess and hope for the best.
       fallback_step_size = kRelativeStepSize;
     }

     // 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);

       double residuals[num_residuals];  // NOLINT
       if (!function->Evaluate(parameters, residuals, 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) =
           Map<const ResidualVector>(residuals, num_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, NULL)) {
           // Something went wrong; bail.
           return false;
         }
         parameter_jacobian.col(j) -=
             Map<ResidualVector>(residuals, num_residuals, 1);
         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;
   }
 };

 // Prevent invalid instantiations.
 template <typename CostFunctionNoJacobian,
           int num_residuals,
           int parameter_block,
           NumericDiffMethod method>
 struct Differencer<CostFunctionNoJacobian,
                   num_residuals,
                   0 /* parameter_block_size */,
                   parameter_block,
                   method> {
   static bool EvaluateJacobianForParameterBlock(
       const CostFunctionNoJacobian *function,
       double const* residuals_at_eval_point,
       double **parameters,
       double **jacobians) {
     LOG(FATAL) << "Shouldn't get here.";
     return true;
   }
 };

 template <typename CostFunctionNoJacobian,
          NumericDiffMethod method = CENTRAL, int M = 0,
          int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5 = 0>
 class NumericDiffCostFunction
     : public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> {
  public:
   NumericDiffCostFunction(CostFunctionNoJacobian* function,
                           Ownership ownership)
       : function_(function), ownership_(ownership) {}

   virtual ~NumericDiffCostFunction() {
     if (ownership_ != TAKE_OWNERSHIP) {
       function_.release();
     }
   }

   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;
     }

     // Create a copy of the parameters which will get mutated.
     const int kParametersSize = N0 + N1 + N2 + N3 + N4 + N5;
     double parameters_copy[kParametersSize];
     double *parameters_references_copy[6];
     parameters_references_copy[0] = &parameters_copy[0];
     parameters_references_copy[1] = &parameters_copy[0] + N0;
     parameters_references_copy[2] = &parameters_copy[0] + N0 + N1;
     parameters_references_copy[3] = &parameters_copy[0] + N0 + N1 + N2;
     parameters_references_copy[4] = &parameters_copy[0] + N0 + N1 + N2 + N3;
     parameters_references_copy[5] =
         &parameters_copy[0] + N0 + N1 + N2 + N3 + N4;

 #define COPY_PARAMETER_BLOCK(block) \
     if (N ## block) memcpy(parameters_references_copy[block], \
                            parameters[block], \
                            sizeof(double) * N ## block);  // NOLINT
     COPY_PARAMETER_BLOCK(0);
     COPY_PARAMETER_BLOCK(1);
     COPY_PARAMETER_BLOCK(2);
     COPY_PARAMETER_BLOCK(3);
     COPY_PARAMETER_BLOCK(4);
     COPY_PARAMETER_BLOCK(5);
 #undef COPY_PARAMETER_BLOCK

 #define EVALUATE_JACOBIAN_FOR_BLOCK(block) \
     if (N ## block && jacobians[block]) { \
       if (!Differencer<CostFunctionNoJacobian, /* NOLINT */ \
                        M, \
                        N ## block, \
                        block, \
                        method>::EvaluateJacobianForParameterBlock( \
           function_.get(), \
           residuals, \
           parameters_references_copy, \
           jacobians)) { \
         return false; \
       } \
     }
     EVALUATE_JACOBIAN_FOR_BLOCK(0);
     EVALUATE_JACOBIAN_FOR_BLOCK(1);
     EVALUATE_JACOBIAN_FOR_BLOCK(2);
     EVALUATE_JACOBIAN_FOR_BLOCK(3);
     EVALUATE_JACOBIAN_FOR_BLOCK(4);
     EVALUATE_JACOBIAN_FOR_BLOCK(5);
 #undef EVALUATE_JACOBIAN_FOR_BLOCK
     return true;
   }

  private:
   internal::scoped_ptr<CostFunctionNoJacobian> function_;
   Ownership ownership_;
 };

 }  // namespace ceres

 #endif  // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
	// 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)
	//
	// Create CostFunctions as needed by the least squares framework with jacobians
	// computed via numeric (a.k.a. finite) differentiation. For more details see
	// http://en.wikipedia.org/wiki/Numerical_differentiation.
	//
	// To get a numerically differentiated cost function, define a subclass of
	// CostFunction such that the Evaluate() function ignores the jacobian
	// parameter. The numeric differentiation wrapper will fill in the jacobian
	// parameter if nececssary by repeatedly calling the Evaluate() function with
	// small changes to the appropriate parameters, and computing the slope. For
	// performance, the numeric differentiation wrapper class is templated on the
	// concrete cost function, even though it could be implemented only in terms of
	// the virtual CostFunction interface.
	//
	// The numerically differentiated version of a cost function for a cost function
	// can be constructed as follows:
	//
	// CostFunction* cost_function
	// = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>(
	// new MyCostFunction(...), TAKE_OWNERSHIP);
	//
	// where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8
	// respectively. Look at the tests for a more detailed example.
	//
	// The central difference method is considerably more accurate at the cost of
	// twice as many function evaluations than forward difference. Consider using
	// central differences begin with, and only after that works, trying forward
	// difference to improve performance.
	//
	// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives.

	#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
	#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_

	#include <cstring>
	#include <glog/logging.h>
	#include "Eigen/Dense"
	#include "ceres/internal/scoped_ptr.h"
	#include "ceres/sized_cost_function.h"
	#include "ceres/types.h"

	namespace ceres {

	enum NumericDiffMethod {
	CENTRAL,
	FORWARD
	};

	// 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 CostFunctionNoJacobian,
	int num_residuals,
	int parameter_block_size,
	int parameter_block,
	NumericDiffMethod method>
	struct Differencer {
	// Mutates parameters but must restore them before return.
	static bool EvaluateJacobianForParameterBlock(
	const CostFunctionNoJacobian *function,
	double const* residuals_at_eval_point,
	double **parameters,
	double **jacobians) {
	using Eigen::Map;
	using Eigen::Matrix;
	using Eigen::RowMajor;
	using Eigen::ColMajor;

	typedef Matrix<double, num_residuals, 1> ResidualVector;
	typedef Matrix<double, parameter_block_size, 1> ParameterVector;
	typedef Matrix<double, num_residuals, parameter_block_size,
	(parameter_block_size == 1 &&
	num_residuals > 1) ? ColMajor : RowMajor> JacobianMatrix;

	Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block],
	num_residuals,
	parameter_block_size);

	// Mutate 1 element at a time and then restore.
	Map<ParameterVector> x_plus_delta(parameters[parameter_block],
	parameter_block_size);
	ParameterVector x(x_plus_delta);

	// TODO(keir): Pick a smarter number! In 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.
	const double kRelativeStepSize = 1e-6;

	ParameterVector step_size = x.array().abs() * kRelativeStepSize;

	// To handle cases where a parameter 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. Take
	// kRelativeStepSize as a guess and hope for the best.
	fallback_step_size = kRelativeStepSize;
	}

	// 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);

	double residuals[num_residuals]; // NOLINT
	if (!function->Evaluate(parameters, residuals, 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) =
	Map<const ResidualVector>(residuals, num_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, NULL)) {
	// Something went wrong; bail.
	return false;
	}
	parameter_jacobian.col(j) -=
	Map<ResidualVector>(residuals, num_residuals, 1);
	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;
	}
	};

	// Prevent invalid instantiations.
	template <typename CostFunctionNoJacobian,
	int num_residuals,
	int parameter_block,
	NumericDiffMethod method>
	struct Differencer<CostFunctionNoJacobian,
	num_residuals,
	0 /* parameter_block_size */,
	parameter_block,
	method> {
	static bool EvaluateJacobianForParameterBlock(
	const CostFunctionNoJacobian *function,
	double const* residuals_at_eval_point,
	double **parameters,
	double **jacobians) {
	LOG(FATAL) << "Shouldn't get here.";
	return true;
	}
	};

	template <typename CostFunctionNoJacobian,
	NumericDiffMethod method = CENTRAL, int M = 0,
	int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5 = 0>
	class NumericDiffCostFunction
	: public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> {
	public:
	NumericDiffCostFunction(CostFunctionNoJacobian* function,
	Ownership ownership)
	: function_(function), ownership_(ownership) {}

	virtual ~NumericDiffCostFunction() {
	if (ownership_ != TAKE_OWNERSHIP) {
	function_.release();
	}
	}

	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;
	}

	// Create a copy of the parameters which will get mutated.
	const int kParametersSize = N0 + N1 + N2 + N3 + N4 + N5;
	double parameters_copy[kParametersSize];
	double *parameters_references_copy[6];
	parameters_references_copy[0] = &parameters_copy[0];
	parameters_references_copy[1] = &parameters_copy[0] + N0;
	parameters_references_copy[2] = &parameters_copy[0] + N0 + N1;
	parameters_references_copy[3] = &parameters_copy[0] + N0 + N1 + N2;
	parameters_references_copy[4] = &parameters_copy[0] + N0 + N1 + N2 + N3;
	parameters_references_copy[5] =
	&parameters_copy[0] + N0 + N1 + N2 + N3 + N4;

	#define COPY_PARAMETER_BLOCK(block) \
	if (N ## block) memcpy(parameters_references_copy[block], \
	parameters[block], \
	sizeof(double) * N ## block); // NOLINT
	COPY_PARAMETER_BLOCK(0);
	COPY_PARAMETER_BLOCK(1);
	COPY_PARAMETER_BLOCK(2);
	COPY_PARAMETER_BLOCK(3);
	COPY_PARAMETER_BLOCK(4);
	COPY_PARAMETER_BLOCK(5);
	#undef COPY_PARAMETER_BLOCK

	#define EVALUATE_JACOBIAN_FOR_BLOCK(block) \
	if (N ## block && jacobians[block]) { \
	if (!Differencer<CostFunctionNoJacobian, /* NOLINT */ \
	M, \
	N ## block, \
	block, \
	method>::EvaluateJacobianForParameterBlock( \
	function_.get(), \
	residuals, \
	parameters_references_copy, \
	jacobians)) { \
	return false; \
	} \
	}
	EVALUATE_JACOBIAN_FOR_BLOCK(0);
	EVALUATE_JACOBIAN_FOR_BLOCK(1);
	EVALUATE_JACOBIAN_FOR_BLOCK(2);
	EVALUATE_JACOBIAN_FOR_BLOCK(3);
	EVALUATE_JACOBIAN_FOR_BLOCK(4);
	EVALUATE_JACOBIAN_FOR_BLOCK(5);
	#undef EVALUATE_JACOBIAN_FOR_BLOCK
	return true;
	}

	private:
	internal::scoped_ptr<CostFunctionNoJacobian> function_;
	Ownership ownership_;
	};

	} // namespace ceres

	#endif // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_