NumericDiffFunctor.

A wrapper class that takes a variadic functor evaluating a
function, numerically differentiates it and makes it available as a
templated functor so that it can be easily used as part of Ceres'
automatic differentiation framework.

The tests for NumericDiffCostFunction and NumericDiffFunctor have
a lot of stuff that is common, so refactor them to reduce code.

Change-Id: I83b01e58b05e575fb2530d15cbd611928298646a

include/ceres/numeric_diff_functor.h

@@ -0,0 +1,341 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 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: sameeragarwal@google.com (Sameer Agarwal)
+//
+// A wrapper class that takes a variadic functor evaluating a
+// function, numerically differentiates it and makes it available as a
+// templated functor so that it can be easily used as part of Ceres'
+// automatic differentiation framework.
+//
+// For example:
+//
+// For example, let us assume that
+//
+//  struct IntrinsicProjection
+//    IntrinsicProjection(const double* observations);
+//    bool operator()(const double* calibration,
+//                    const double* point,
+//                    double* residuals);
+//  };
+//
+// is a functor that implements the projection of a point in its local
+// coordinate system onto its image plane and subtracts it from the
+// observed point projection.
+//
+// Now we would like to compose the action of this functor with the
+// action of camera extrinsics, i.e., rotation and translation, which
+// is given by the following templated function
+//
+//   template<typename T>
+//   void RotateAndTranslatePoint(const T* rotation,
+//                                const T* translation,
+//                                const T* point,
+//                                T* result);
+//
+// To compose the extrinsics and intrinsics, we can construct a
+// CameraProjection functor as follows.
+//
+// struct CameraProjection {
+//    typedef NumericDiffFunctor<IntrinsicProjection, CENTRAL, 2, 5, 3>
+//       IntrinsicProjectionFunctor;
+//
+//   CameraProjection(double* observation) {
+//     intrinsic_projection_.reset(
+//         new IntrinsicProjectionFunctor(observation)) {
+//   }
+//
+//   template <typename T>
+//   bool operator(const T* rotation,
+//                 const T* translation,
+//                 const T* intrinsics,
+//                 const T* point,
+//                 T* residuals) const {
+//     T transformed_point[3];
+//     RotateAndTranslatePoint(rotation, translation, point, transformed_point);
+//     return (*intrinsic_projection_)(intrinsics, transformed_point, residual);
+//   }
+//
+//  private:
+//   scoped_ptr<IntrinsicProjectionFunctor> intrinsic_projection_;
+// };
+//
+// Here, we made the choice of using CENTRAL differences to compute
+// the jacobian of IntrinsicProjection.
+//
+// Now, we are ready to construct an automatically differentiated cost
+// function as
+//
+// CostFunction* cost_function =
+//    new AutoDiffCostFunction<CameraProjection, 2, 3, 3, 5>(
+//        new CameraProjection(observations));
+//
+// cost_function now seamlessly integrates automatic differentiation
+// of RotateAndTranslatePoint with a numerically differentiated
+// version of IntrinsicProjection.
+
+#include "ceres/numeric_diff_cost_function.h"
+#include "ceres/types.h"
+#include "ceres/cost_function_to_functor.h"
+
+namespace ceres {
+
+template<typename Functor,
+         NumericDiffMethod kMethod = CENTRAL,
+         int kNumResiduals = 0,
+         int N0 = 0, int N1 = 0 , int N2 = 0, int N3 = 0, int N4 = 0,
+         int N5 = 0, int N6 = 0 , int N7 = 0, int N8 = 0, int N9 = 0>
+class NumericDiffFunctor {
+ public:
+  // relative_step_size controls the step size used by the numeric
+  // differentiation process.
+  NumericDiffFunctor(double relative_step_size = 1e-6)
+      : functor_(new NumericDiffCostFunction<Functor,
+                                             kMethod,
+                                             kNumResiduals,
+                                             N0, N1, N2, N3, N4,
+                                             N5, N6, N7, N8, N9>(
+                                                 new Functor, relative_step_size)) {
+  }
+
+  NumericDiffFunctor(Functor* functor, double relative_step_size = 1e-6)
+      : functor_(new NumericDiffCostFunction<Functor,
+                                             kMethod,
+                                             kNumResiduals,
+                                             N0, N1, N2, N3, N4,
+                                             N5, N6, N7, N8, N9>(
+                                                 functor, relative_step_size)) {
+  }
+
+  bool operator()(const double* x0, double* residuals) const {
+    functor_(x0, residuals);
+  }
+
+  bool operator()(const double* x0,
+                  const double* x1,
+                  double* residuals) const {
+    return functor_(x0, x1, residuals);
+  }
+
+  bool operator()(const double* x0,
+                  const double* x1,
+                  const double* x2,
+                  double* residuals) const {
+    return functor_(x0, x1, x2, residuals);
+  }
+
+  bool operator()(const double* x0,
+                  const double* x1,
+                  const double* x2,
+                  const double* x3,
+                  double* residuals) const {
+    return functor_(x0, x1, x2, x3, residuals);
+  }
+
+  bool operator()(const double* x0,
+                  const double* x1,
+                  const double* x2,
+                  const double* x3,
+                  const double* x4,
+                  double* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, residuals);
+  }
+
+  bool operator()(const double* x0,
+                  const double* x1,
+                  const double* x2,
+                  const double* x3,
+                  const double* x4,
+                  const double* x5,
+                  double* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, x5, residuals);
+  }
+
+  bool operator()(const double* x0,
+                  const double* x1,
+                  const double* x2,
+                  const double* x3,
+                  const double* x4,
+                  const double* x5,
+                  const double* x6,
+                  double* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, x5, x6, residuals);
+  }
+
+  bool operator()(const double* x0,
+                  const double* x1,
+                  const double* x2,
+                  const double* x3,
+                  const double* x4,
+                  const double* x5,
+                  const double* x6,
+                  const double* x7,
+                  double* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, x5, x6, x7, residuals);
+  }
+
+  bool operator()(const double* x0,
+                  const double* x1,
+                  const double* x2,
+                  const double* x3,
+                  const double* x4,
+                  const double* x5,
+                  const double* x6,
+                  const double* x7,
+                  const double* x8,
+                  double* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, residuals);
+  }
+
+  bool operator()(const double* x0,
+                  const double* x1,
+                  const double* x2,
+                  const double* x3,
+                  const double* x4,
+                  const double* x5,
+                  const double* x6,
+                  const double* x7,
+                  const double* x8,
+                  const double* x9,
+                  double* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, residuals);
+  }
+
+  template <typename T>
+  bool operator()(const T* x0, T* residuals) const {
+    functor_(x0, residuals);
+  }
+
+  template <typename T>
+  bool operator()(const T* x0,
+                  const T* x1,
+                  T* residuals) const {
+    return functor_(x0, x1, residuals);
+  }
+
+  template <typename T>
+  bool operator()(const T* x0,
+                  const T* x1,
+                  const T* x2,
+                  T* residuals) const {
+    return functor_(x0, x1, x2, residuals);
+  }
+
+  template <typename T>
+  bool operator()(const T* x0,
+                  const T* x1,
+                  const T* x2,
+                  const T* x3,
+                  T* residuals) const {
+    return functor_(x0, x1, x2, x3, residuals);
+  }
+
+  template <typename T>
+  bool operator()(const T* x0,
+                  const T* x1,
+                  const T* x2,
+                  const T* x3,
+                  const T* x4,
+                  T* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, residuals);
+  }
+
+  template <typename T>
+  bool operator()(const T* x0,
+                  const T* x1,
+                  const T* x2,
+                  const T* x3,
+                  const T* x4,
+                  const T* x5,
+                  T* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, x5, residuals);
+  }
+
+  template <typename T>
+  bool operator()(const T* x0,
+                  const T* x1,
+                  const T* x2,
+                  const T* x3,
+                  const T* x4,
+                  const T* x5,
+                  const T* x6,
+                  T* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, x5, x6, residuals);
+  }
+
+  template <typename T>
+  bool operator()(const T* x0,
+                  const T* x1,
+                  const T* x2,
+                  const T* x3,
+                  const T* x4,
+                  const T* x5,
+                  const T* x6,
+                  const T* x7,
+                  T* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, x5, x6, x7, residuals);
+  }
+
+  template <typename T>
+  bool operator()(const T* x0,
+                  const T* x1,
+                  const T* x2,
+                  const T* x3,
+                  const T* x4,
+                  const T* x5,
+                  const T* x6,
+                  const T* x7,
+                  const T* x8,
+                  T* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, residuals);
+  }
+
+  template <typename T>
+  bool operator()(const T* x0,
+                  const T* x1,
+                  const T* x2,
+                  const T* x3,
+                  const T* x4,
+                  const T* x5,
+                  const T* x6,
+                  const T* x7,
+                  const T* x8,
+                  const T* x9,
+                  T* residuals) const {
+    return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, residuals);
+  }
+
+
+ private:
+  CostFunctionToFunctor<kNumResiduals,
+                        N0, N1, N2, N3, N4,
+                        N5, N6, N7, N8, N9> functor_;
+
+};
+
+}  // namespace ceres

internal/ceres/CMakeLists.txt

@@ -201,7 +201,7 @@ ENDIF (NOT MSVC AND NOT ${BUILD_ANDROID})
 
 IF (${BUILD_TESTING} AND ${GFLAGS})
   ADD_LIBRARY(gtest gmock_gtest_all.cc gmock_main.cc)
-  ADD_LIBRARY(test_util test_util.cc)
+  ADD_LIBRARY(test_util test_util.cc numeric_diff_test_utils.cc)
   TARGET_LINK_LIBRARIES(gtest ${GFLAGS_LIB} ${GLOG_LIB})
 
   MACRO (CERES_TEST NAME)
@@ -240,6 +240,7 @@ IF (${BUILD_TESTING} AND ${GFLAGS})
   CERES_TEST(minimizer)
   CERES_TEST(normal_prior)
   CERES_TEST(numeric_diff_cost_function)
+  CERES_TEST(numeric_diff_functor)
   CERES_TEST(ordered_groups)
   CERES_TEST(parameter_block)
   CERES_TEST(parameter_block_ordering)

internal/ceres/numeric_diff_cost_function_test.cc

@@ -36,8 +36,7 @@
 #include <vector>
 #include "ceres/internal/macros.h"
 #include "ceres/internal/scoped_ptr.h"
-#include "ceres/sized_cost_function.h"
-#include "ceres/stringprintf.h"
+#include "ceres/numeric_diff_test_utils.h"
 #include "ceres/test_util.h"
 #include "ceres/types.h"
 #include "glog/logging.h"
@@ -46,224 +45,110 @@
 namespace ceres {
 namespace internal {
 
-// y1 = x1'x2      -> dy1/dx1 = x2,               dy1/dx2 = x1
-// y2 = (x1'x2)^2  -> dy2/dx1 = 2 * x2 * (x1'x2), dy2/dx2 = 2 * x1 * (x1'x2)
-// y3 = x2'x2      -> dy3/dx1 = 0,                dy3/dx2 = 2 * x2
-struct EasyFunctor {
-  bool operator()(const double* x1, const double* x2, double* residuals) const {
-    residuals[0] = residuals[1] = residuals[2] = 0;
-    for (int i = 0; i < 5; ++i) {
-      residuals[0] += x1[i] * x2[i];
-      residuals[2] += x2[i] * x2[i];
-    }
-    residuals[1] = residuals[0] * residuals[0];
-    return true;
-  }
-};
+TEST(NumericDiffCostFunction, EasyCaseFunctorCentralDifferences) {
+  internal::scoped_ptr<CostFunction> cost_function;
+  cost_function.reset(
+      new NumericDiffCostFunction<EasyFunctor,
+                                  CENTRAL,
+                                  3,  /* number of residuals */
+                                  5,  /* size of x1 */
+                                  5   /* size of x2 */>(
+          new EasyFunctor));
+  EasyFunctor functor;
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL);
+}
 
-class EasyCostFunction : public SizedCostFunction<3, 5, 5> {
- public:
-  virtual bool Evaluate(double const* const* parameters,
-                        double* residuals,
-                        double** jacobians) const {
-    (void) jacobians;  // Ignored.
-    return EasyFunctor()(parameters[0], parameters[1], residuals);
-  }
-};
+TEST(NumericDiffCostFunction, EasyCaseFunctorForwardDifferences) {
+  internal::scoped_ptr<CostFunction> cost_function;
+  cost_function.reset(
+      new NumericDiffCostFunction<EasyFunctor,
+                                  FORWARD,
+                                  3,  /* number of residuals */
+                                  5,  /* size of x1 */
+                                  5   /* size of x2 */>(
+          new EasyFunctor));
+  EasyFunctor functor;
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD);
+}
 
-TEST(NumericDiffCostFunction, EasyCase) {
-  // Try both central and forward difference.
-  internal::scoped_ptr<CostFunction> cfs[4];
-  cfs[0].reset(
+TEST(NumericDiffCostFunction, EasyCaseCostFunctionCentralDifferences) {
+  internal::scoped_ptr<CostFunction> cost_function;
+  cost_function.reset(
       new NumericDiffCostFunction<EasyCostFunction,
                                   CENTRAL,
                                   3,  /* number of residuals */
                                   5,  /* size of x1 */
                                   5   /* size of x2 */>(
           new EasyCostFunction, TAKE_OWNERSHIP));
+  EasyFunctor functor;
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL);
+}
 
-  cfs[1].reset(
+TEST(NumericDiffCostFunction, EasyCaseCostFunctionForwardDifferences) {
+  internal::scoped_ptr<CostFunction> cost_function;
+  cost_function.reset(
       new NumericDiffCostFunction<EasyCostFunction,
                                   FORWARD,
                                   3,  /* number of residuals */
                                   5,  /* size of x1 */
                                   5   /* size of x2 */>(
           new EasyCostFunction, TAKE_OWNERSHIP));
-
-    cfs[2].reset(
-        new NumericDiffCostFunction< EasyFunctor,
-                                     CENTRAL,
-                                     3,  /* number of residuals */
-                                     5,  /* size of x1 */
-                                     5   /* size of x2 */>(
-                                         new EasyFunctor));
-
-    cfs[3].reset(
-        new NumericDiffCostFunction< EasyFunctor,
-                                     FORWARD,
-                                     3,  /* number of residuals */
-                                     5,  /* size of x1 */
-                                     5   /* size of x2 */>(
-                                         new EasyFunctor));
-
-
-  for (int c = 0; c < 4; ++c) {
-    CostFunction *cost_function = cfs[c].get();
-
-    double x1[] = { 1.0, 2.0, 3.0, 4.0, 5.0 };
-    double x2[] = { 9.0, 9.0, 5.0, 5.0, 1.0 };
-    double *parameters[] = { &x1[0], &x2[0] };
-
-    double dydx1[15];  // 3 x 5, row major.
-    double dydx2[15];  // 3 x 5, row major.
-    double *jacobians[2] = { &dydx1[0], &dydx2[0] };
-
-    double residuals[3] = {-1e-100, -2e-100, -3e-100 };
-
-    ASSERT_TRUE(cost_function->Evaluate(&parameters[0],
-                                        &residuals[0],
-                                        &jacobians[0]));
-
-    EXPECT_EQ(residuals[0], 67);
-    EXPECT_EQ(residuals[1], 4489);
-    EXPECT_EQ(residuals[2], 213);
-
-    for (int i = 0; i < 5; ++i) {
-      LOG(INFO) << "c = " << c << " i = " << i;
-      const double kEps = c == 0 ? /* central */ 3e-9 : /* forward */ 2e-5;
-
-      ExpectClose(x2[i],                    dydx1[5 * 0 + i], kEps);  // y1
-      ExpectClose(x1[i],                    dydx2[5 * 0 + i], kEps);
-      ExpectClose(2 * x2[i] * residuals[0], dydx1[5 * 1 + i], kEps);  // y2
-      ExpectClose(2 * x1[i] * residuals[0], dydx2[5 * 1 + i], kEps);
-      ExpectClose(0.0,                      dydx1[5 * 2 + i], kEps);  // y3
-      ExpectClose(2 * x2[i],                dydx2[5 * 2 + i], kEps);
-    }
-  }
+  EasyFunctor functor;
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD);
 }
 
-// y1 = sin(x1'x2)
-// y2 = exp(-x1'x2 / 10)
-//
-// dy1/dx1 =  x2 * cos(x1'x2),            dy1/dx2 =  x1 * cos(x1'x2)
-// dy2/dx1 = -x2 * exp(-x1'x2 / 10) / 10, dy2/dx2 = -x2 * exp(-x1'x2 / 10) / 10
-struct TranscendentalFunctor {
-  bool operator()(const double* x1, const double* x2, double* residuals) const {
-    double x1x2 = 0;
-    for (int i = 0; i < 5; ++i) {
-      x1x2 += x1[i] * x2[i];
-    }
-    residuals[0] = sin(x1x2);
-    residuals[1] = exp(-x1x2 / 10);
-    return true;
-  }
-};
-
-class TranscendentalTestCostFunction : public SizedCostFunction<2, 5, 5> {
- public:
-  virtual bool Evaluate(double const* const* parameters,
-                        double* residuals,
-                        double** jacobians) const {
-    (void) jacobians;  // Ignored.
-    return TranscendentalFunctor()(parameters[0], parameters[1], residuals);
-  }
-};
-
-TEST(NumericDiffCostFunction, TransendentalOperationsInCostFunction) {
-  // Try both central and forward difference.
-  internal::scoped_ptr<CostFunction> cfs[4];
-  cfs[0].reset(
-      new NumericDiffCostFunction<TranscendentalTestCostFunction,
+TEST(NumericDiffCostFunction, TranscendentalCaseFunctorCentralDifferences) {
+  internal::scoped_ptr<CostFunction> cost_function;
+  cost_function.reset(
+      new NumericDiffCostFunction<TranscendentalFunctor,
                                   CENTRAL,
                                   2,  /* number of residuals */
                                   5,  /* size of x1 */
                                   5   /* size of x2 */>(
-          new TranscendentalTestCostFunction, TAKE_OWNERSHIP));
+          new TranscendentalFunctor));
+  TranscendentalFunctor functor;
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL);
+}
 
-  cfs[1].reset(
-      new NumericDiffCostFunction<TranscendentalTestCostFunction,
+TEST(NumericDiffCostFunction, TranscendentalCaseFunctorForwardDifferences) {
+  internal::scoped_ptr<CostFunction> cost_function;
+  cost_function.reset(
+      new NumericDiffCostFunction<TranscendentalFunctor,
                                   FORWARD,
                                   2,  /* number of residuals */
                                   5,  /* size of x1 */
                                   5   /* size of x2 */>(
-          new TranscendentalTestCostFunction, TAKE_OWNERSHIP));
+          new TranscendentalFunctor));
+  TranscendentalFunctor functor;
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD);
+}
 
-  cfs[2].reset(
-      new NumericDiffCostFunction<TranscendentalFunctor,
+TEST(NumericDiffCostFunction, TranscendentalCaseCostFunctionCentralDifferences) {
+  internal::scoped_ptr<CostFunction> cost_function;
+  cost_function.reset(
+      new NumericDiffCostFunction<TranscendentalCostFunction,
                                   CENTRAL,
                                   2,  /* number of residuals */
                                   5,  /* size of x1 */
                                   5   /* size of x2 */>(
-                                      new TranscendentalFunctor));
+          new TranscendentalCostFunction, TAKE_OWNERSHIP));
+  TranscendentalFunctor functor;
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL);
+}
 
-  cfs[3].reset(
-      new NumericDiffCostFunction<TranscendentalFunctor,
+TEST(NumericDiffCostFunction, TranscendentalCaseCostFunctionForwardDifferences) {
+  internal::scoped_ptr<CostFunction> cost_function;
+  cost_function.reset(
+      new NumericDiffCostFunction<TranscendentalCostFunction,
                                   FORWARD,
                                   2,  /* number of residuals */
                                   5,  /* size of x1 */
                                   5   /* size of x2 */>(
-                                      new TranscendentalFunctor));
-
-  for (int c = 0; c < 4; ++c) {
-    CostFunction *cost_function = cfs[c].get();
-
-    struct {
-      double x1[5];
-      double x2[5];
-    } kTests[] = {
-      { { 1.0, 2.0, 3.0, 4.0, 5.0 },  // No zeros.
-        { 9.0, 9.0, 5.0, 5.0, 1.0 },
-      },
-      { { 0.0, 2.0, 3.0, 0.0, 5.0 },  // Some zeros x1.
-        { 9.0, 9.0, 5.0, 5.0, 1.0 },
-      },
-      { { 1.0, 2.0, 3.0, 1.0, 5.0 },  // Some zeros x2.
-        { 0.0, 9.0, 0.0, 5.0, 0.0 },
-      },
-      { { 0.0, 0.0, 0.0, 0.0, 0.0 },  // All zeros x1.
-        { 9.0, 9.0, 5.0, 5.0, 1.0 },
-      },
-      { { 1.0, 2.0, 3.0, 4.0, 5.0 },  // All zeros x2.
-        { 0.0, 0.0, 0.0, 0.0, 0.0 },
-      },
-      { { 0.0, 0.0, 0.0, 0.0, 0.0 },  // All zeros.
-        { 0.0, 0.0, 0.0, 0.0, 0.0 },
-      },
-    };
-    for (int k = 0; k < CERES_ARRAYSIZE(kTests); ++k) {
-      double *x1 = &(kTests[k].x1[0]);
-      double *x2 = &(kTests[k].x2[0]);
-      double *parameters[] = { x1, x2 };
-
-      double dydx1[10];
-      double dydx2[10];
-      double *jacobians[2] = { &dydx1[0], &dydx2[0] };
-
-      double residuals[2];
-
-      ASSERT_TRUE(cost_function->Evaluate(&parameters[0],
-                                          &residuals[0],
-                                          &jacobians[0]));
-      LOG(INFO) << "Ran evaluate for test k=" << k << " c=" << c;
-
-      double x1x2 = 0;
-      for (int i = 0; i < 5; ++i) {
-        x1x2 += x1[i] * x2[i];
-      }
-
-      for (int i = 0; i < 5; ++i) {
-        const double kEps = c == 0 ? /* central */ 3e-9 : /* forward */ 2e-5;
-
-        ExpectClose( x2[i] * cos(x1x2),              dydx1[5 * 0 + i], kEps);
-        ExpectClose( x1[i] * cos(x1x2),              dydx2[5 * 0 + i], kEps);
-        ExpectClose(-x2[i] * exp(-x1x2 / 10.) / 10., dydx1[5 * 1 + i], kEps);
-        ExpectClose(-x1[i] * exp(-x1x2 / 10.) / 10., dydx2[5 * 1 + i], kEps);
-      }
-    }
-  }
+          new TranscendentalCostFunction, TAKE_OWNERSHIP));
+  TranscendentalFunctor functor;
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD);
 }
 
-
 template<int num_rows, int num_cols>
 class SizeTestingCostFunction : public SizedCostFunction<num_rows, num_cols> {
  public:

internal/ceres/numeric_diff_functor_test.cc

@@ -0,0 +1,123 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 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: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/numeric_diff_functor.h"
+
+#include <algorithm>
+#include <cmath>
+#include <string>
+#include <vector>
+#include "ceres/autodiff_cost_function.h"
+#include "ceres/internal/scoped_ptr.h"
+#include "ceres/numeric_diff_test_utils.h"
+#include "ceres/test_util.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+TEST(NumericDiffCostFunction, EasyCaseCentralDifferences) {
+  typedef NumericDiffFunctor<EasyFunctor, CENTRAL, 3, 5, 5> NumericDiffEasyFunctor;
+
+  internal::scoped_ptr<CostFunction> cost_function;
+  EasyFunctor functor;
+
+  cost_function.reset(
+      new AutoDiffCostFunction<NumericDiffEasyFunctor, 3, 5, 5>(
+          new NumericDiffEasyFunctor));
+
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL);
+
+  cost_function.reset(
+      new AutoDiffCostFunction<NumericDiffEasyFunctor, 3, 5, 5>(
+          new NumericDiffEasyFunctor(new EasyFunctor)));
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL);
+}
+
+TEST(NumericDiffCostFunction, EasyCaseForwardDifferences) {
+  typedef NumericDiffFunctor<EasyFunctor, FORWARD, 3, 5, 5> NumericDiffEasyFunctor;
+
+  internal::scoped_ptr<CostFunction> cost_function;
+  EasyFunctor functor;
+
+  cost_function.reset(
+      new AutoDiffCostFunction<NumericDiffEasyFunctor, 3, 5, 5>(
+          new NumericDiffEasyFunctor));
+
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD);
+
+  cost_function.reset(
+      new AutoDiffCostFunction<NumericDiffEasyFunctor, 3, 5, 5>(
+          new NumericDiffEasyFunctor(new EasyFunctor)));
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD);
+}
+
+TEST(NumericDiffCostFunction, TranscendentalCaseCentralDifferences) {
+  typedef NumericDiffFunctor<TranscendentalFunctor, CENTRAL, 2, 5, 5>
+      NumericDiffTranscendentalFunctor;
+
+  internal::scoped_ptr<CostFunction> cost_function;
+  TranscendentalFunctor functor;
+
+  cost_function.reset(
+      new AutoDiffCostFunction<NumericDiffTranscendentalFunctor, 2, 5, 5>(
+          new NumericDiffTranscendentalFunctor));
+
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL);
+
+  cost_function.reset(
+      new AutoDiffCostFunction<NumericDiffTranscendentalFunctor, 2, 5, 5>(
+          new NumericDiffTranscendentalFunctor(new TranscendentalFunctor)));
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL);
+}
+
+TEST(NumericDiffCostFunction, TranscendentalCaseForwardDifferences) {
+  typedef NumericDiffFunctor<TranscendentalFunctor, FORWARD, 2, 5, 5>
+      NumericDiffTranscendentalFunctor;
+
+  internal::scoped_ptr<CostFunction> cost_function;
+  TranscendentalFunctor functor;
+
+  cost_function.reset(
+      new AutoDiffCostFunction<NumericDiffTranscendentalFunctor, 2, 5, 5>(
+          new NumericDiffTranscendentalFunctor));
+
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD);
+
+  cost_function.reset(
+      new AutoDiffCostFunction<NumericDiffTranscendentalFunctor, 2, 5, 5>(
+          new NumericDiffTranscendentalFunctor(new TranscendentalFunctor)));
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD);
+}
+
+}  // namespace internal
+}  // namespace ceres

internal/ceres/numeric_diff_test_utils.cc

@@ -0,0 +1,130 @@
+#include "ceres/numeric_diff_test_utils.h"
+
+#include <algorithm>
+#include <cmath>
+#include "ceres/cost_function.h"
+#include "ceres/internal/macros.h"
+#include "ceres/test_util.h"
+#include "ceres/types.h"
+#include "gtest/gtest.h"
+
+
+namespace ceres {
+namespace internal {
+
+bool EasyFunctor::operator()(const double* x1,
+                             const double* x2,
+                             double* residuals) const {
+  residuals[0] = residuals[1] = residuals[2] = 0;
+  for (int i = 0; i < 5; ++i) {
+    residuals[0] += x1[i] * x2[i];
+    residuals[2] += x2[i] * x2[i];
+  }
+  residuals[1] = residuals[0] * residuals[0];
+  return true;
+}
+
+void EasyFunctor::ExpectCostFunctionEvaluationIsNearlyCorrect(
+    const CostFunction& cost_function,
+    NumericDiffMethod method) const {
+  double x1[] = { 1.0, 2.0, 3.0, 4.0, 5.0 };
+  double x2[] = { 9.0, 9.0, 5.0, 5.0, 1.0 };
+  double *parameters[] = { &x1[0], &x2[0] };
+
+  double dydx1[15];  // 3 x 5, row major.
+  double dydx2[15];  // 3 x 5, row major.
+  double *jacobians[2] = { &dydx1[0], &dydx2[0] };
+
+  double residuals[3] = {-1e-100, -2e-100, -3e-100 };
+
+  ASSERT_TRUE(cost_function.Evaluate(&parameters[0],
+                                     &residuals[0],
+                                     &jacobians[0]));
+
+  EXPECT_EQ(residuals[0], 67);
+  EXPECT_EQ(residuals[1], 4489);
+  EXPECT_EQ(residuals[2], 213);
+
+  const double tolerance = (method == CENTRAL)? 3e-9 : 2e-5;
+
+  for (int i = 0; i < 5; ++i) {
+    ExpectClose(x2[i],                    dydx1[5 * 0 + i], tolerance);  // y1
+    ExpectClose(x1[i],                    dydx2[5 * 0 + i], tolerance);
+    ExpectClose(2 * x2[i] * residuals[0], dydx1[5 * 1 + i], tolerance);  // y2
+    ExpectClose(2 * x1[i] * residuals[0], dydx2[5 * 1 + i], tolerance);
+    ExpectClose(0.0,                      dydx1[5 * 2 + i], tolerance);  // y3
+    ExpectClose(2 * x2[i],                dydx2[5 * 2 + i], tolerance);
+  }
+}
+
+bool TranscendentalFunctor::operator()(const double* x1,
+                                       const double* x2,
+                                       double* residuals) const {
+  double x1x2 = 0;
+  for (int i = 0; i < 5; ++i) {
+    x1x2 += x1[i] * x2[i];
+  }
+  residuals[0] = sin(x1x2);
+  residuals[1] = exp(-x1x2 / 10);
+  return true;
+}
+
+void TranscendentalFunctor::ExpectCostFunctionEvaluationIsNearlyCorrect(
+    const CostFunction& cost_function,
+    NumericDiffMethod method) const {
+  struct {
+    double x1[5];
+    double x2[5];
+  } kTests[] = {
+    { { 1.0, 2.0, 3.0, 4.0, 5.0 },  // No zeros.
+      { 9.0, 9.0, 5.0, 5.0, 1.0 },
+    },
+    { { 0.0, 2.0, 3.0, 0.0, 5.0 },  // Some zeros x1.
+      { 9.0, 9.0, 5.0, 5.0, 1.0 },
+    },
+    { { 1.0, 2.0, 3.0, 1.0, 5.0 },  // Some zeros x2.
+      { 0.0, 9.0, 0.0, 5.0, 0.0 },
+    },
+    { { 0.0, 0.0, 0.0, 0.0, 0.0 },  // All zeros x1.
+      { 9.0, 9.0, 5.0, 5.0, 1.0 },
+    },
+    { { 1.0, 2.0, 3.0, 4.0, 5.0 },  // All zeros x2.
+      { 0.0, 0.0, 0.0, 0.0, 0.0 },
+    },
+    { { 0.0, 0.0, 0.0, 0.0, 0.0 },  // All zeros.
+      { 0.0, 0.0, 0.0, 0.0, 0.0 },
+    },
+  };
+
+  for (int k = 0; k < CERES_ARRAYSIZE(kTests); ++k) {
+    double *x1 = &(kTests[k].x1[0]);
+    double *x2 = &(kTests[k].x2[0]);
+    double *parameters[] = { x1, x2 };
+
+    double dydx1[10];
+    double dydx2[10];
+    double *jacobians[2] = { &dydx1[0], &dydx2[0] };
+
+    double residuals[2];
+
+    ASSERT_TRUE(cost_function.Evaluate(&parameters[0],
+                                       &residuals[0],
+                                       &jacobians[0]));
+    double x1x2 = 0;
+    for (int i = 0; i < 5; ++i) {
+      x1x2 += x1[i] * x2[i];
+    }
+
+    const double tolerance = (method == CENTRAL)? 3e-9 : 2e-5;
+
+    for (int i = 0; i < 5; ++i) {
+      ExpectClose( x2[i] * cos(x1x2),              dydx1[5 * 0 + i], tolerance);
+      ExpectClose( x1[i] * cos(x1x2),              dydx2[5 * 0 + i], tolerance);
+      ExpectClose(-x2[i] * exp(-x1x2 / 10.) / 10., dydx1[5 * 1 + i], tolerance);
+      ExpectClose(-x1[i] * exp(-x1x2 / 10.) / 10., dydx2[5 * 1 + i], tolerance);
+    }
+  }
+}
+
+}  // namespace internal
+}  // namespace ceres

internal/ceres/numeric_diff_test_utils.h

@@ -0,0 +1,56 @@
+#include "ceres/cost_function.h"
+#include "ceres/sized_cost_function.h"
+#include "ceres/types.h"
+
+namespace ceres {
+namespace internal {
+
+// y1 = x1'x2      -> dy1/dx1 = x2,               dy1/dx2 = x1
+// y2 = (x1'x2)^2  -> dy2/dx1 = 2 * x2 * (x1'x2), dy2/dx2 = 2 * x1 * (x1'x2)
+// y3 = x2'x2      -> dy3/dx1 = 0,                dy3/dx2 = 2 * x2
+class EasyFunctor {
+ public:
+  bool operator()(const double* x1, const double* x2, double* residuals) const;
+  void ExpectCostFunctionEvaluationIsNearlyCorrect(
+      const CostFunction& cost_function,
+      NumericDiffMethod method) const;
+};
+
+class EasyCostFunction : public SizedCostFunction<3, 5, 5> {
+ public:
+  virtual bool Evaluate(double const* const* parameters,
+                        double* residuals,
+                        double** /* not used */) const {
+    return functor_(parameters[0], parameters[1], residuals);
+  }
+
+ private:
+  EasyFunctor functor_;
+};
+
+// y1 = sin(x1'x2)
+// y2 = exp(-x1'x2 / 10)
+//
+// dy1/dx1 =  x2 * cos(x1'x2),            dy1/dx2 =  x1 * cos(x1'x2)
+// dy2/dx1 = -x2 * exp(-x1'x2 / 10) / 10, dy2/dx2 = -x2 * exp(-x1'x2 / 10) / 10
+class TranscendentalFunctor {
+ public:
+  bool operator()(const double* x1, const double* x2, double* residuals) const;
+  void ExpectCostFunctionEvaluationIsNearlyCorrect(
+      const CostFunction& cost_function,
+      NumericDiffMethod method) const;
+};
+
+class TranscendentalCostFunction : public SizedCostFunction<2, 5, 5> {
+ public:
+  virtual bool Evaluate(double const* const* parameters,
+                        double* residuals,
+                        double** /* not used */) const {
+    return functor_(parameters[0], parameters[1], residuals);
+  }
+ private:
+  TranscendentalFunctor functor_;
+};
+
+}  // namespace internal
+}  // namespace ceres