Add the GradientChecker.

The GradientChecker is a utility class written by
William Rucklidge that can be used to check that the
derivatives returned by a cost function match those
returned by numerically differentiating the residuals
returned by the same cost function.

This is useful when developing CostFunction objects
and testing them before plugging them into an optimization
problem.

Change-Id: Ic60f859b48b6246406448555d25556784e097b81

include/ceres/gradient_checker.h

@@ -0,0 +1,221 @@
+// 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.
+// Copyright 2007 Google Inc. All Rights Reserved.
+//
+// Author: wjr@google.com (William Rucklidge)
+//
+// This file contains a class that exercises a cost function, to make sure
+// that it is computing reasonable derivatives. It compares the Jacobians
+// computed by the cost function with those obtained by finite
+// differences.
+
+#ifndef CERES_PUBLIC_GRADIENT_CHECKER_H_
+#define CERES_PUBLIC_GRADIENT_CHECKER_H_
+
+#include <algorithm>
+#include <cstddef>
+#include <vector>
+
+#include <glog/logging.h>
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/internal/macros.h"
+#include "ceres/internal/scoped_ptr.h"
+#include "ceres/numeric_diff_cost_function.h"
+
+namespace ceres {
+
+// An object that exercises a cost function, to compare the answers that it
+// gives with derivatives estimated using finite differencing.
+//
+// The only likely usage of this is for testing.
+//
+// How to use: Fill in an array of pointers to parameter blocks for your
+// CostFunction, and then call Probe(). Check that the return value is
+// 'true'. See prober_test.cc for an example.
+//
+// This is templated similarly to NumericDiffCostFunction, as it internally
+// uses that.
+template <typename CostFunctionToProbe,
+          int M = 0, int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0>
+class GradientChecker {
+ public:
+  // Here we stash some results from the probe, for later
+  // inspection.
+  struct GradientCheckResults {
+    // Computed cost.
+    Vector cost;
+
+    // The sizes of these matrices are dictated by the cost function's
+    // parameter and residual block sizes. Each vector's length will
+    // term->parameter_block_sizes().size(), and each matrix is the
+    // Jacobian of the residual with respect to the corresponding parameter
+    // block.
+
+    // Derivatives as computed by the cost function.
+    vector<Matrix> term_jacobians;
+
+    // Derivatives as computed by finite differencing.
+    vector<Matrix> finite_difference_jacobians;
+
+    // Infinity-norm of term_jacobians - finite_difference_jacobians.
+    double error_jacobians;
+  };
+
+  // Checks the Jacobian computed by a cost function.
+  //
+  // probe_point: The parameter values at which to probe.
+  // error_tolerance: A threshold for the infinity-norm difference
+  // between the Jacobians. If the Jacobians differ by more than
+  // this amount, then the probe fails.
+  //
+  // term: The cost function to test. Not retained after this call returns.
+  //
+  // results: On return, the two Jacobians (and other information)
+  // will be stored here.  May be NULL.
+  //
+  // Returns true if no problems are detected and the difference between the
+  // Jacobians is less than error_tolerance.
+  static bool Probe(double const* const* probe_point,
+                    double error_tolerance,
+                    CostFunctionToProbe *term,
+                    GradientCheckResults* results) {
+    CHECK_NOTNULL(probe_point);
+    CHECK_NOTNULL(term);
+    LOG(INFO) << "-------------------- Starting Probe() --------------------";
+
+    // We need a GradientCheckeresults, whether or not they supplied one.
+    internal::scoped_ptr<GradientCheckResults> owned_results;
+    if (results == NULL) {
+      owned_results.reset(new GradientCheckResults);
+      results = owned_results.get();
+    }
+
+    // Do a consistency check between the term and the template parameters.
+    CHECK_EQ(M, term->num_residuals());
+    const int num_residuals = M;
+    const vector<int16>& block_sizes = term->parameter_block_sizes();
+    const int num_blocks = block_sizes.size();
+
+    CHECK_LE(num_blocks, 5) << "Unable to test functions that take more "
+                            << "than 5 parameter blocks";
+    if (N0) {
+      CHECK_EQ(N0, block_sizes[0]);
+      CHECK_GE(num_blocks, 1);
+    } else {
+      CHECK_LT(num_blocks, 1);
+    }
+    if (N1) {
+      CHECK_EQ(N1, block_sizes[1]);
+      CHECK_GE(num_blocks, 2);
+    } else {
+      CHECK_LT(num_blocks, 2);
+    }
+    if (N2) {
+      CHECK_EQ(N2, block_sizes[2]);
+      CHECK_GE(num_blocks, 3);
+    } else {
+      CHECK_LT(num_blocks, 3);
+    }
+    if (N3) {
+      CHECK_EQ(N3, block_sizes[3]);
+      CHECK_GE(num_blocks, 4);
+    } else {
+      CHECK_LT(num_blocks, 4);
+    }
+    if (N4) {
+      CHECK_EQ(N4, block_sizes[4]);
+      CHECK_GE(num_blocks, 5);
+    } else {
+      CHECK_LT(num_blocks, 5);
+    }
+
+    results->term_jacobians.clear();
+    results->term_jacobians.resize(num_blocks);
+    results->finite_difference_jacobians.clear();
+    results->finite_difference_jacobians.resize(num_blocks);
+
+    internal::FixedArray<double*> term_jacobian_pointers(num_blocks);
+    internal::FixedArray<double*> finite_difference_jacobian_pointers(num_blocks);
+    for (int i = 0; i < num_blocks; i++) {
+      results->term_jacobians[i].resize(num_residuals, block_sizes[i]);
+      term_jacobian_pointers[i] = results->term_jacobians[i].data();
+      results->finite_difference_jacobians[i].resize(
+          num_residuals, block_sizes[i]);
+      finite_difference_jacobian_pointers[i] =
+          results->finite_difference_jacobians[i].data();
+    }
+    results->cost.resize(num_residuals, 1);
+
+    CHECK(term->Evaluate(probe_point, results->cost.data(),
+                         term_jacobian_pointers.get()));
+    NumericDiffCostFunction<CostFunctionToProbe, CENTRAL, M, N0, N1, N2, N3, N4>
+        numeric_term(term, DO_NOT_TAKE_OWNERSHIP);
+    CHECK(numeric_term.Evaluate(probe_point, results->cost.data(),
+                                finite_difference_jacobian_pointers.get()));
+
+    results->error_jacobians = 0;
+    for (int i = 0; i < num_blocks; i++) {
+      Matrix jacobian_difference = results->term_jacobians[i] -
+          results->finite_difference_jacobians[i];
+      results->error_jacobians =
+          std::max(results->error_jacobians,
+                   jacobian_difference.lpNorm<Eigen::Infinity>());
+    }
+
+    LOG(INFO) << "========== term-computed derivatives ==========";
+    for (int i = 0; i < num_blocks; i++) {
+      LOG(INFO) << "term_computed block " << i;
+      LOG(INFO) << "\n" << results->term_jacobians[i];
+    }
+
+    LOG(INFO) << "========== finite-difference derivatives ==========";
+    for (int i = 0; i < num_blocks; i++) {
+      LOG(INFO) << "finite_difference block " << i;
+      LOG(INFO) << "\n" << results->finite_difference_jacobians[i];
+    }
+
+    LOG(INFO) << "========== difference ==========";
+    for (int i = 0; i < num_blocks; i++) {
+      LOG(INFO) << "difference block " << i;
+      LOG(INFO) << (results->term_jacobians[i] -
+                    results->finite_difference_jacobians[i]);
+    }
+
+    LOG(INFO) << "||difference|| = " << results->error_jacobians;
+
+    return results->error_jacobians < error_tolerance;
+  }
+
+ private:
+  CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(GradientChecker);
+};
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_GRADIENT_CHECKER_H_

internal/ceres/CMakeLists.txt

@@ -220,6 +220,7 @@ IF (${BUILD_TESTING} AND ${GFLAGS})
   CERES_TEST(corrector)
   CERES_TEST(dense_sparse_matrix)
   CERES_TEST(evaluator)
+  CERES_TEST(gradient_checker)
   CERES_TEST(gradient_checking_cost_function)
   CERES_TEST(graph)
   CERES_TEST(graph_algorithms)

internal/ceres/gradient_checker_test.cc

@@ -0,0 +1,193 @@
+// 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: wjr@google.com (William Rucklidge)
+//
+// This file contains tests for the GradientChecker class.
+
+#include "ceres/gradient_checker.h"
+
+#include <cmath>
+#include <cstdlib>
+#include <glog/logging.h>
+#include <vector>
+
+#include "ceres/cost_function.h"
+#include "ceres/random.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+// We pick a (non-quadratic) function whose derivative are easy:
+//
+//    f = exp(- a' x).
+//   df = - f a.
+//
+// where 'a' is a vector of the same size as 'x'. In the block
+// version, they are both block vectors, of course.
+class GoodTestTerm : public CostFunction {
+ public:
+  GoodTestTerm(int arity, int const *dim) : arity_(arity) {
+    // Make 'arity' random vectors.
+    a_.resize(arity_);
+    for (int j = 0; j < arity_; ++j) {
+      a_[j].resize(dim[j]);
+      for (int u = 0; u < dim[j]; ++u) {
+        a_[j][u] = 2.0 * RandDouble() - 1.0;
+      }
+    }
+
+    for (int i = 0; i < arity_; i++) {
+      mutable_parameter_block_sizes()->push_back(dim[i]);
+    }
+    set_num_residuals(1);
+  }
+
+  bool Evaluate(double const* const* parameters,
+                double* residuals,
+                double** jacobians) const {
+    // Compute a . x.
+    double ax = 0;
+    for (int j = 0; j < arity_; ++j) {
+      for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
+        ax += a_[j][u] * parameters[j][u];
+      }
+    }
+
+    // This is the cost, but also appears as a factor
+    // in the derivatives.
+    double f = *residuals = exp(-ax);
+
+    // Accumulate 1st order derivatives.
+    if (jacobians) {
+      for (int j = 0; j < arity_; ++j) {
+        if (jacobians[j]) {
+          for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
+            // See comments before class.
+            jacobians[j][u] = - f * a_[j][u];
+          }
+        }
+      }
+    }
+
+    return true;
+  }
+
+ private:
+  int arity_;
+  vector<vector<double> > a_;  // our vectors.
+};
+
+class BadTestTerm : public CostFunction {
+ public:
+  BadTestTerm(int arity, int const *dim) : arity_(arity) {
+    // Make 'arity' random vectors.
+    a_.resize(arity_);
+    for (int j = 0; j < arity_; ++j) {
+      a_[j].resize(dim[j]);
+      for (int u = 0; u < dim[j]; ++u) {
+        a_[j][u] = 2.0 * RandDouble() - 1.0;
+      }
+    }
+
+    for (int i = 0; i < arity_; i++) {
+      mutable_parameter_block_sizes()->push_back(dim[i]);
+    }
+    set_num_residuals(1);
+  }
+
+  bool Evaluate(double const* const* parameters,
+                double* residuals,
+                double** jacobians) const {
+    // Compute a . x.
+    double ax = 0;
+    for (int j = 0; j < arity_; ++j) {
+      for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
+        ax += a_[j][u] * parameters[j][u];
+      }
+    }
+
+    // This is the cost, but also appears as a factor
+    // in the derivatives.
+    double f = *residuals = exp(-ax);
+
+    // Accumulate 1st order derivatives.
+    if (jacobians) {
+      for (int j = 0; j < arity_; ++j) {
+        if (jacobians[j]) {
+          for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
+            // See comments before class.
+            jacobians[j][u] = - f * a_[j][u] + 0.001;
+          }
+        }
+      }
+    }
+
+    return true;
+  }
+
+ private:
+  int arity_;
+  vector<vector<double> > a_;  // our vectors.
+};
+
+TEST(GradientChecker, SmokeTest) {
+  srand(5);
+
+  // Test with 3 blocks of size 2, 3 and 4.
+  int const arity = 3;
+  int const dim[arity] = { 2, 3, 4 };
+
+  // Make a random set of blocks.
+  FixedArray<double*> parameters(arity);
+  for (int j = 0; j < arity; ++j) {
+    parameters[j] = new double[dim[j]];
+    for (int u = 0; u < dim[j]; ++u) {
+      parameters[j][u] = 2.0 * RandDouble() - 1.0;
+    }
+  }
+
+  // Make a term and probe it.
+  GoodTestTerm good_term(arity, dim);
+  typedef GradientChecker<GoodTestTerm, 1, 2, 3, 4> GoodTermGradientChecker;
+  EXPECT_TRUE(GoodTermGradientChecker::Probe(
+      parameters.get(), 1e-6, &good_term, NULL));
+
+  BadTestTerm bad_term(arity, dim);
+  typedef GradientChecker<BadTestTerm, 1, 2, 3, 4> BadTermGradientChecker;
+  EXPECT_FALSE(BadTermGradientChecker::Probe(
+      parameters.get(), 1e-6, &bad_term, NULL));
+
+  for (int j = 0; j < arity; j++) {
+    delete[] parameters[j];
+  }
+}
+
+}  // namespace internal
+}  // namespace ceres