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

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