numeric_diff_cost_function.h revision 0ae28bd5885b5daa526898fcf7c323dc2c3e1963
1// Ceres Solver - A fast non-linear least squares minimizer 2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3// http://code.google.com/p/ceres-solver/ 4// 5// Redistribution and use in source and binary forms, with or without 6// modification, are permitted provided that the following conditions are met: 7// 8// * Redistributions of source code must retain the above copyright notice, 9// this list of conditions and the following disclaimer. 10// * Redistributions in binary form must reproduce the above copyright notice, 11// this list of conditions and the following disclaimer in the documentation 12// and/or other materials provided with the distribution. 13// * Neither the name of Google Inc. nor the names of its contributors may be 14// used to endorse or promote products derived from this software without 15// specific prior written permission. 16// 17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27// POSSIBILITY OF SUCH DAMAGE. 28// 29// Author: keir@google.com (Keir Mierle) 30// 31// Create CostFunctions as needed by the least squares framework with jacobians 32// computed via numeric (a.k.a. finite) differentiation. For more details see 33// http://en.wikipedia.org/wiki/Numerical_differentiation. 34// 35// To get a numerically differentiated cost function, define a subclass of 36// CostFunction such that the Evaluate() function ignores the jacobian 37// parameter. The numeric differentiation wrapper will fill in the jacobian 38// parameter if nececssary by repeatedly calling the Evaluate() function with 39// small changes to the appropriate parameters, and computing the slope. For 40// performance, the numeric differentiation wrapper class is templated on the 41// concrete cost function, even though it could be implemented only in terms of 42// the virtual CostFunction interface. 43// 44// The numerically differentiated version of a cost function for a cost function 45// can be constructed as follows: 46// 47// CostFunction* cost_function 48// = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>( 49// new MyCostFunction(...), TAKE_OWNERSHIP); 50// 51// where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8 52// respectively. Look at the tests for a more detailed example. 53// 54// The central difference method is considerably more accurate at the cost of 55// twice as many function evaluations than forward difference. Consider using 56// central differences begin with, and only after that works, trying forward 57// difference to improve performance. 58// 59// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives. 60 61#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ 62#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ 63 64#include <cstring> 65#include <glog/logging.h> 66#include "Eigen/Dense" 67#include "ceres/internal/scoped_ptr.h" 68#include "ceres/sized_cost_function.h" 69#include "ceres/types.h" 70 71namespace ceres { 72 73enum NumericDiffMethod { 74 CENTRAL, 75 FORWARD 76}; 77 78// This is split from the main class because C++ doesn't allow partial template 79// specializations for member functions. The alternative is to repeat the main 80// class for differing numbers of parameters, which is also unfortunate. 81template <typename CostFunctionNoJacobian, 82 int num_residuals, 83 int parameter_block_size, 84 int parameter_block, 85 NumericDiffMethod method> 86struct Differencer { 87 // Mutates parameters but must restore them before return. 88 static bool EvaluateJacobianForParameterBlock( 89 const CostFunctionNoJacobian *function, 90 double const* residuals_at_eval_point, 91 double **parameters, 92 double **jacobians) { 93 using Eigen::Map; 94 using Eigen::Matrix; 95 using Eigen::RowMajor; 96 using Eigen::ColMajor; 97 98 typedef Matrix<double, num_residuals, 1> ResidualVector; 99 typedef Matrix<double, parameter_block_size, 1> ParameterVector; 100 typedef Matrix<double, num_residuals, parameter_block_size, 101 (parameter_block_size == 1 && 102 num_residuals > 1) ? ColMajor : RowMajor> JacobianMatrix; 103 104 Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block], 105 num_residuals, 106 parameter_block_size); 107 108 // Mutate 1 element at a time and then restore. 109 Map<ParameterVector> x_plus_delta(parameters[parameter_block], 110 parameter_block_size); 111 ParameterVector x(x_plus_delta); 112 113 // TODO(keir): Pick a smarter number! In theory a good choice is sqrt(eps) * 114 // x, which for doubles means about 1e-8 * x. However, I have found this 115 // number too optimistic. This number should be exposed for users to change. 116 const double kRelativeStepSize = 1e-6; 117 118 ParameterVector step_size = x.array().abs() * kRelativeStepSize; 119 120 // To handle cases where a parameter is exactly zero, instead use the mean 121 // step_size for the other dimensions. 122 double fallback_step_size = step_size.sum() / step_size.rows(); 123 if (fallback_step_size == 0.0) { 124 // If all the parameters are zero, there's no good answer. Take 125 // kRelativeStepSize as a guess and hope for the best. 126 fallback_step_size = kRelativeStepSize; 127 } 128 129 // For each parameter in the parameter block, use finite differences to 130 // compute the derivative for that parameter. 131 for (int j = 0; j < parameter_block_size; ++j) { 132 if (step_size(j) == 0.0) { 133 // The parameter is exactly zero, so compromise and use the mean 134 // step_size from the other parameters. This can break in many cases, 135 // but it's hard to pick a good number without problem specific 136 // knowledge. 137 step_size(j) = fallback_step_size; 138 } 139 x_plus_delta(j) = x(j) + step_size(j); 140 141 double residuals[num_residuals]; // NOLINT 142 if (!function->Evaluate(parameters, residuals, NULL)) { 143 // Something went wrong; bail. 144 return false; 145 } 146 147 // Compute this column of the jacobian in 3 steps: 148 // 1. Store residuals for the forward part. 149 // 2. Subtract residuals for the backward (or 0) part. 150 // 3. Divide out the run. 151 parameter_jacobian.col(j) = 152 Map<const ResidualVector>(residuals, num_residuals); 153 154 double one_over_h = 1 / step_size(j); 155 if (method == CENTRAL) { 156 // Compute the function on the other side of x(j). 157 x_plus_delta(j) = x(j) - step_size(j); 158 159 if (!function->Evaluate(parameters, residuals, NULL)) { 160 // Something went wrong; bail. 161 return false; 162 } 163 parameter_jacobian.col(j) -= 164 Map<ResidualVector>(residuals, num_residuals, 1); 165 one_over_h /= 2; 166 } else { 167 // Forward difference only; reuse existing residuals evaluation. 168 parameter_jacobian.col(j) -= 169 Map<const ResidualVector>(residuals_at_eval_point, num_residuals); 170 } 171 x_plus_delta(j) = x(j); // Restore x_plus_delta. 172 173 // Divide out the run to get slope. 174 parameter_jacobian.col(j) *= one_over_h; 175 } 176 return true; 177 } 178}; 179 180// Prevent invalid instantiations. 181template <typename CostFunctionNoJacobian, 182 int num_residuals, 183 int parameter_block, 184 NumericDiffMethod method> 185struct Differencer<CostFunctionNoJacobian, 186 num_residuals, 187 0 /* parameter_block_size */, 188 parameter_block, 189 method> { 190 static bool EvaluateJacobianForParameterBlock( 191 const CostFunctionNoJacobian *function, 192 double const* residuals_at_eval_point, 193 double **parameters, 194 double **jacobians) { 195 LOG(FATAL) << "Shouldn't get here."; 196 return true; 197 } 198}; 199 200template <typename CostFunctionNoJacobian, 201 NumericDiffMethod method = CENTRAL, int M = 0, 202 int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5 = 0> 203class NumericDiffCostFunction 204 : public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> { 205 public: 206 NumericDiffCostFunction(CostFunctionNoJacobian* function, 207 Ownership ownership) 208 : function_(function), ownership_(ownership) {} 209 210 virtual ~NumericDiffCostFunction() { 211 if (ownership_ != TAKE_OWNERSHIP) { 212 function_.release(); 213 } 214 } 215 216 virtual bool Evaluate(double const* const* parameters, 217 double* residuals, 218 double** jacobians) const { 219 // Get the function value (residuals) at the the point to evaluate. 220 bool success = function_->Evaluate(parameters, residuals, NULL); 221 if (!success) { 222 // Something went wrong; ignore the jacobian. 223 return false; 224 } 225 if (!jacobians) { 226 // Nothing to do; just forward. 227 return true; 228 } 229 230 // Create a copy of the parameters which will get mutated. 231 const int kParametersSize = N0 + N1 + N2 + N3 + N4 + N5; 232 double parameters_copy[kParametersSize]; 233 double *parameters_references_copy[6]; 234 parameters_references_copy[0] = ¶meters_copy[0]; 235 parameters_references_copy[1] = ¶meters_copy[0] + N0; 236 parameters_references_copy[2] = ¶meters_copy[0] + N0 + N1; 237 parameters_references_copy[3] = ¶meters_copy[0] + N0 + N1 + N2; 238 parameters_references_copy[4] = ¶meters_copy[0] + N0 + N1 + N2 + N3; 239 parameters_references_copy[5] = 240 ¶meters_copy[0] + N0 + N1 + N2 + N3 + N4; 241 242#define COPY_PARAMETER_BLOCK(block) \ 243 if (N ## block) memcpy(parameters_references_copy[block], \ 244 parameters[block], \ 245 sizeof(double) * N ## block); // NOLINT 246 COPY_PARAMETER_BLOCK(0); 247 COPY_PARAMETER_BLOCK(1); 248 COPY_PARAMETER_BLOCK(2); 249 COPY_PARAMETER_BLOCK(3); 250 COPY_PARAMETER_BLOCK(4); 251 COPY_PARAMETER_BLOCK(5); 252#undef COPY_PARAMETER_BLOCK 253 254#define EVALUATE_JACOBIAN_FOR_BLOCK(block) \ 255 if (N ## block && jacobians[block]) { \ 256 if (!Differencer<CostFunctionNoJacobian, /* NOLINT */ \ 257 M, \ 258 N ## block, \ 259 block, \ 260 method>::EvaluateJacobianForParameterBlock( \ 261 function_.get(), \ 262 residuals, \ 263 parameters_references_copy, \ 264 jacobians)) { \ 265 return false; \ 266 } \ 267 } 268 EVALUATE_JACOBIAN_FOR_BLOCK(0); 269 EVALUATE_JACOBIAN_FOR_BLOCK(1); 270 EVALUATE_JACOBIAN_FOR_BLOCK(2); 271 EVALUATE_JACOBIAN_FOR_BLOCK(3); 272 EVALUATE_JACOBIAN_FOR_BLOCK(4); 273 EVALUATE_JACOBIAN_FOR_BLOCK(5); 274#undef EVALUATE_JACOBIAN_FOR_BLOCK 275 return true; 276 } 277 278 private: 279 internal::scoped_ptr<CostFunctionNoJacobian> function_; 280 Ownership ownership_; 281}; 282 283} // namespace ceres 284 285#endif // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ 286