10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer
20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/
40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//
50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without
60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met:
70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//
80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice,
90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//   this list of conditions and the following disclaimer.
100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice,
110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//   this list of conditions and the following disclaimer in the documentation
120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//   and/or other materials provided with the distribution.
130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be
140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//   used to endorse or promote products derived from this software without
150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//   specific prior written permission.
160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//
170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE.
280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//
290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: keir@google.com (Keir Mierle)
301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         sameeragarwal@google.com (Sameer Agarwal)
310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//
320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Create CostFunctions as needed by the least squares framework with jacobians
330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// computed via numeric (a.k.a. finite) differentiation. For more details see
340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://en.wikipedia.org/wiki/Numerical_differentiation.
350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//
361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// To get an numerically differentiated cost function, you must define
371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// a class with a operator() (a functor) that computes the residuals.
381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// The function must write the computed value in the last argument
401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// (the only non-const one) and return true to indicate success.
411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Please see cost_function.h for details on how the return value
421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// maybe used to impose simple constraints on the parameter block.
431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// For example, consider a scalar error e = k - x'y, where both x and y are
451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// two-dimensional column vector parameters, the prime sign indicates
461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// transposition, and k is a constant. The form of this error, which is the
471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// difference between a constant and an expression, is a common pattern in least
481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// squares problems. For example, the value x'y might be the model expectation
491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for a series of measurements, where there is an instance of the cost function
501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for each measurement k.
511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// The actual cost added to the total problem is e^2, or (k - x'k)^2; however,
531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// the squaring is implicitly done by the optimization framework.
541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// To write an numerically-differentiable cost function for the above model, first
561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// define the object
571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   class MyScalarCostFunctor {
591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     MyScalarCostFunctor(double k): k_(k) {}
601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     bool operator()(const double* const x,
621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                     const double* const y,
631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                     double* residuals) const {
641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//       residuals[0] = k_ - x[0] * y[0] + x[1] * y[1];
651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//       return true;
661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     }
671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//    private:
691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     double k_;
701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   };
711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Note that in the declaration of operator() the input parameters x
731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// and y come first, and are passed as const pointers to arrays of
741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// doubles. If there were three input parameters, then the third input
751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameter would come after y. The output is always the last
761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameter, and is also a pointer to an array. In the example above,
771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// the residual is a scalar, so only residuals[0] is set.
781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Then given this class definition, the numerically differentiated
801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cost function with central differences used for computing the
811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// derivative can be constructed as follows.
821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   CostFunction* cost_function
841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//       = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, 1, 2, 2>(
851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//           new MyScalarCostFunctor(1.0));                    ^     ^  ^  ^
861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                                             |     |  |  |
871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                 Finite Differencing Scheme -+     |  |  |
881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                 Dimension of residual ------------+  |  |
891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                 Dimension of x ----------------------+  |
901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                 Dimension of y -------------------------+
911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// In this example, there is usually an instance for each measurement of k.
931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// In the instantiation above, the template parameters following
951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing
961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// a 1-dimensional output from two arguments, both 2-dimensional.
971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
9879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// NumericDiffCostFunction also supports cost functions with a
9979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// runtime-determined number of residuals. For example:
10079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//
10179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//   CostFunction* cost_function
10279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//       = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>(
10379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//           new CostFunctorWithDynamicNumResiduals(1.0),               ^     ^  ^
10479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//           TAKE_OWNERSHIP,                                            |     |  |
10579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//           runtime_number_of_residuals); <----+                       |     |  |
10679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//                                              |                       |     |  |
10779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//                                              |                       |     |  |
10879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//             Actual number of residuals ------+                       |     |  |
10979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//             Indicate dynamic number of residuals --------------------+     |  |
11079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//             Dimension of x ------------------------------------------------+  |
11179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//             Dimension of y ---------------------------------------------------+
11279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez//
1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// The framework can currently accommodate cost functions of up to 10
1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// independent variables, and there is no limit on the dimensionality
1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// of each of them.
1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// The central difference method is considerably more accurate at the cost of
1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// twice as many function evaluations than forward difference. Consider using
1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// central differences begin with, and only after that works, trying forward
1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// difference to improve performance.
1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// WARNING #1: A common beginner's error when first using
1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// NumericDiffCostFunction is to get the sizing wrong. In particular,
1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// there is a tendency to set the template parameters to (dimension of
1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// residual, number of parameters) instead of passing a dimension
1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameter for *every parameter*. In the example above, that would
1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// be <MyScalarCostFunctor, 1, 2>, which is missing the last '2'
1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// argument. Please be careful when setting the size parameters.
1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling////////////////////////////////////////////////////////////////////////////
1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling////////////////////////////////////////////////////////////////////////////
1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ALTERNATE INTERFACE
1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// For a variety of reason, including compatibility with legacy code,
1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// NumericDiffCostFunction can also take CostFunction objects as
1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// input. The following describes how.
1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//
1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// To get a numerically differentiated cost function, define a
1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// subclass of CostFunction such that the Evaluate() function ignores
1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// the jacobian parameter. The numeric differentiation wrapper will
1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// fill in the jacobian parameter if necessary by repeatedly calling
1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// the Evaluate() function with small changes to the appropriate
1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameters, and computing the slope. For performance, the numeric
1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// differentiation wrapper class is templated on the concrete cost
1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// function, even though it could be implemented only in terms of the
1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// virtual CostFunction interface.
1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//
1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The numerically differentiated version of a cost function for a cost function
1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// can be constructed as follows:
1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//
1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//   CostFunction* cost_function
1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//       = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>(
1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//           new MyCostFunction(...), TAKE_OWNERSHIP);
1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//
1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8
1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// respectively. Look at the tests for a more detailed example.
1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong//
1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives.
1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "Eigen/Dense"
1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/cost_function.h"
1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/numeric_diff.h"
1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h"
1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/sized_cost_function.h"
1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h"
1701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h"
1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres {
1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename CostFunctor,
1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          NumericDiffMethod method = CENTRAL,
1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          int kNumResiduals = 0,  // Number of residuals, or ceres::DYNAMIC
1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          int N0 = 0,   // Number of parameters in block 0.
1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          int N1 = 0,   // Number of parameters in block 1.
1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          int N2 = 0,   // Number of parameters in block 2.
1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          int N3 = 0,   // Number of parameters in block 3.
1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          int N4 = 0,   // Number of parameters in block 4.
1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          int N5 = 0,   // Number of parameters in block 5.
1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          int N6 = 0,   // Number of parameters in block 6.
1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          int N7 = 0,   // Number of parameters in block 7.
1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          int N8 = 0,   // Number of parameters in block 8.
1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling          int N9 = 0>   // Number of parameters in block 9.
1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass NumericDiffCostFunction
1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    : public SizedCostFunction<kNumResiduals,
1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                               N0, N1, N2, N3, N4,
1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                               N5, N6, N7, N8, N9> {
1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public:
1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling  NumericDiffCostFunction(CostFunctor* functor,
19379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez                          Ownership ownership = TAKE_OWNERSHIP,
19479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez                          int num_residuals = kNumResiduals,
1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                          const double relative_step_size = 1e-6)
1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling      :functor_(functor),
19779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez       ownership_(ownership),
19879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez       relative_step_size_(relative_step_size) {
19979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez    if (kNumResiduals == DYNAMIC) {
20079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez      SizedCostFunction<kNumResiduals,
20179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez                        N0, N1, N2, N3, N4,
20279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez                        N5, N6, N7, N8, N9>
20379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez          ::set_num_residuals(num_residuals);
20479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez    }
20579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez  }
2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling  ~NumericDiffCostFunction() {
2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    if (ownership_ != TAKE_OWNERSHIP) {
2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling      functor_.release();
2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    }
2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong  }
2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong  virtual bool Evaluate(double const* const* parameters,
2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong                        double* residuals,
2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong                        double** jacobians) const {
2161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    using internal::FixedArray;
2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    using internal::NumericDiff;
2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    const int kNumParameters = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    const int kNumParameterBlocks =
2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling        (N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) +
2221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling        (N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0);
2231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    // Get the function value (residuals) at the the point to evaluate.
2251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    if (!internal::EvaluateImpl<CostFunctor,
2261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                                N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
2271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                                    functor_.get(),
2281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                                    parameters,
2291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                                    residuals,
2301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                                    functor_.get())) {
2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong      return false;
2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    }
2331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
23479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez    if (jacobians == NULL) {
2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong      return true;
2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    }
2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    // Create a copy of the parameters which will get mutated.
2391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    FixedArray<double> parameters_copy(kNumParameters);
2401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    FixedArray<double*> parameters_reference_copy(kNumParameterBlocks);
2411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    parameters_reference_copy[0] = parameters_copy.get();
2431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    if (N1) parameters_reference_copy[1] = parameters_reference_copy[0] + N0;
2441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    if (N2) parameters_reference_copy[2] = parameters_reference_copy[1] + N1;
2451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    if (N3) parameters_reference_copy[3] = parameters_reference_copy[2] + N2;
2461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    if (N4) parameters_reference_copy[4] = parameters_reference_copy[3] + N3;
2471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    if (N5) parameters_reference_copy[5] = parameters_reference_copy[4] + N4;
2481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    if (N6) parameters_reference_copy[6] = parameters_reference_copy[5] + N5;
2491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    if (N7) parameters_reference_copy[7] = parameters_reference_copy[6] + N6;
2501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    if (N8) parameters_reference_copy[8] = parameters_reference_copy[7] + N7;
2511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    if (N9) parameters_reference_copy[9] = parameters_reference_copy[8] + N8;
2521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define COPY_PARAMETER_BLOCK(block)                                     \
2541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling  if (N ## block) memcpy(parameters_reference_copy[block],              \
2551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                         parameters[block],                             \
2561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                         sizeof(double) * N ## block);  // NOLINT
2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    COPY_PARAMETER_BLOCK(0);
2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    COPY_PARAMETER_BLOCK(1);
2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    COPY_PARAMETER_BLOCK(2);
2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    COPY_PARAMETER_BLOCK(3);
2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    COPY_PARAMETER_BLOCK(4);
2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    COPY_PARAMETER_BLOCK(5);
2641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    COPY_PARAMETER_BLOCK(6);
2651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    COPY_PARAMETER_BLOCK(7);
2661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    COPY_PARAMETER_BLOCK(8);
2671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    COPY_PARAMETER_BLOCK(9);
2681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#undef COPY_PARAMETER_BLOCK
2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
2711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define EVALUATE_JACOBIAN_FOR_BLOCK(block)                              \
2721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    if (N ## block && jacobians[block] != NULL) {                       \
2731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling      if (!NumericDiff<CostFunctor,                                     \
2741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                       method,                                          \
2751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                       kNumResiduals,                                   \
2761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                       N0, N1, N2, N3, N4, N5, N6, N7, N8, N9,          \
2771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                       block,                                           \
2781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                       N ## block >::EvaluateJacobianForParameterBlock( \
2791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                           functor_.get(),                              \
2801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                           residuals,                                   \
2811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                           relative_step_size_,                         \
28279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez                          SizedCostFunction<kNumResiduals,              \
28379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez                           N0, N1, N2, N3, N4,                          \
28479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez                           N5, N6, N7, N8, N9>::num_residuals(),        \
2851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                           parameters_reference_copy.get(),             \
2861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling                           jacobians[block])) {                         \
2871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling        return false;                                                   \
2881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling      }                                                                 \
2890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    }
2901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    EVALUATE_JACOBIAN_FOR_BLOCK(0);
2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    EVALUATE_JACOBIAN_FOR_BLOCK(1);
2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    EVALUATE_JACOBIAN_FOR_BLOCK(2);
2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    EVALUATE_JACOBIAN_FOR_BLOCK(3);
2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    EVALUATE_JACOBIAN_FOR_BLOCK(4);
2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    EVALUATE_JACOBIAN_FOR_BLOCK(5);
2971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    EVALUATE_JACOBIAN_FOR_BLOCK(6);
2981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    EVALUATE_JACOBIAN_FOR_BLOCK(7);
2991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    EVALUATE_JACOBIAN_FOR_BLOCK(8);
3001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling    EVALUATE_JACOBIAN_FOR_BLOCK(9);
3011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
3020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#undef EVALUATE_JACOBIAN_FOR_BLOCK
3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
3040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong    return true;
3050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong  }
3060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
3070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong private:
3081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling  internal::scoped_ptr<CostFunctor> functor_;
3090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong  Ownership ownership_;
3101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling  const double relative_step_size_;
3110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong};
3120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
3130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}  // namespace ceres
3140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
3150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif  // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
316