diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst
index 311f8ae..b82f5ba 100644
--- a/docs/source/nnls_modeling.rst
+++ b/docs/source/nnls_modeling.rst
@@ -873,6 +873,56 @@
    and the Jacobian will be affected appropriately.
 
 
+:class:`GradientChecker`
+================================
+
+.. class:: GradientChecker
+
+    This class compares the Jacobians returned by a cost function against
+    derivatives estimated using finite differencing. It is meant as a tool for
+    unit testing, giving you more fine-grained control than the check_gradients
+    option in the solver options.
+
+    The condition enforced is that
+
+    .. math:: \forall{i,j}: \frac{J_{ij} - J'_{ij}}{max_{ij}(J_{ij} - J'_{ij})} < r
+
+    where :math:`J_{ij}` is the jacobian as computed by the supplied cost
+    function (by the user) multiplied by the local parameterization Jacobian,
+    :math:`J'_{ij}` is the jacobian as computed by finite differences,
+    multiplied by the local parameterization Jacobian as well, and :math:`r`
+    is the relative precision.
+
+   Usage:
+
+   .. code-block:: c++
+
+       //  my_cost_function takes two parameter blocks. The first has a local
+       //  parameterization associated with it.
+       CostFunction* my_cost_function = ...
+       LocalParameterization* my_parameterization = ...
+       NumericDiffOptions numeric_diff_options;
+
+       std::vector<LocalParameterization*> local_parameterizations;
+       local_parameterizations.push_back(my_parameterization);
+       local_parameterizations.push_back(NULL);
+
+       std::vector parameter1;
+       std::vector parameter2;
+       // Fill parameter 1 & 2 with test data...
+
+       std::vector<double*> parameter_blocks;
+       parameter_blocks.push_back(parameter1.data());
+       parameter_blocks.push_back(parameter2.data());
+
+       GradientChecker gradient_checker(my_cost_function,
+           local_parameterizations, numeric_diff_options);
+       GradientCheckResults results;
+       if (!gradient_checker.Probe(parameter_blocks.data(), 1e-9, &results) {
+         LOG(ERROR) << "An error has occured:\n" << results.error_log;
+       }
+
+
 :class:`NormalPrior`
 ====================
 
diff --git a/docs/source/nnls_solving.rst b/docs/source/nnls_solving.rst
index 4eb1fad..ccf43a5 100644
--- a/docs/source/nnls_solving.rst
+++ b/docs/source/nnls_solving.rst
@@ -1559,8 +1559,8 @@
    differences. This is expensive since it involves computing the
    derivative by normal means (e.g. user specified, autodiff, etc),
    then also computing it using finite differences. The results are
-   compared, and if they differ substantially, details are printed to
-   the log.
+   compared, and if they differ substantially, the optimization fails
+   and the details are stored in the solver summary.
 
 .. member:: double Solver::Options::gradient_check_relative_precision
 
diff --git a/include/ceres/gradient_checker.h b/include/ceres/gradient_checker.h
index 2830415..6d285da 100644
--- a/include/ceres/gradient_checker.h
+++ b/include/ceres/gradient_checker.h
@@ -27,194 +27,121 @@
 // 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.
+// Authors: wjr@google.com (William Rucklidge),
+//          keir@google.com (Keir Mierle),
+//          dgossow@google.com (David Gossow)
 
 #ifndef CERES_PUBLIC_GRADIENT_CHECKER_H_
 #define CERES_PUBLIC_GRADIENT_CHECKER_H_
 
-#include <cstddef>
-#include <algorithm>
 #include <vector>
+#include <string>
 
+#include "ceres/cost_function.h"
+#include "ceres/dynamic_numeric_diff_cost_function.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"
+#include "ceres/local_parameterization.h"
 #include "glog/logging.h"
 
 namespace ceres {
 
-// An object that exercises a cost function, to compare the answers that it
-// gives with derivatives estimated using finite differencing.
+// GradientChecker compares the Jacobians returned by a cost function against
+// derivatives estimated using finite differencing.
 //
-// The only likely usage of this is for testing.
+// The condition enforced is that
+//
+//    (J_actual(i, j) - J_numeric(i, j))
+//   ------------------------------------  <  relative_precision
+//   max(J_actual(i, j), J_numeric(i, j))
+//
+// where J_actual(i, j) is the jacobian as computed by the supplied cost
+// function (by the user) multiplied by the local parameterization Jacobian
+// and J_numeric is the jacobian as computed by finite differences, multiplied
+// by the local parameterization Jacobian as well.
 //
 // 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>
+// CostFunction, and then call Probe(). Check that the return value is 'true'.
 class GradientChecker {
  public:
-  // Here we stash some results from the probe, for later
-  // inspection.
-  struct GradientCheckResults {
-    // Computed cost.
-    Vector cost;
+  // This will not take ownership of the cost function or local
+  // parameterizations.
+  //
+  // function: The cost function to probe.
+  // local_parameterization: A vector of local parameterizations for each
+  // parameter. May be NULL or contain NULL pointers to indicate that the
+  // respective parameter does not have a local parameterization.
+  // options: Options to use for numerical differentiation.
+  GradientChecker(
+      const CostFunction* function,
+      const std::vector<const LocalParameterization*>* local_parameterizations,
+      const NumericDiffOptions& options);
 
-    // 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.
+  // Contains results from a call to Probe for later inspection.
+  struct ProbeResults {
+    // The return value of the cost function.
+    bool return_value;
+
+    // Computed residual vector.
+    Vector residuals;
+
+    // The sizes of the Jacobians below are dictated by the cost function's
+    // parameter block size and residual block sizes. If a parameter block
+    // has a local parameterization associated with it, the size of the "local"
+    // Jacobian will be determined by the local parameterization dimension and
+    // residual block size, otherwise it will be identical to the regular
+    // Jacobian.
 
     // Derivatives as computed by the cost function.
-    std::vector<Matrix> term_jacobians;
+    std::vector<Matrix> jacobians;
 
-    // Derivatives as computed by finite differencing.
-    std::vector<Matrix> finite_difference_jacobians;
+    // Derivatives as computed by the cost function in local space.
+    std::vector<Matrix> local_jacobians;
 
-    // Infinity-norm of term_jacobians - finite_difference_jacobians.
-    double error_jacobians;
+    // Derivatives as computed by nuerical differentiation in local space.
+    std::vector<Matrix> numeric_jacobians;
+
+    // Derivatives as computed by nuerical differentiation in local space.
+    std::vector<Matrix> local_numeric_jacobians;
+
+    // Contains the maximum relative error found in the local Jacobians.
+    double maximum_relative_error;
+
+    // If an error was detected, this will contain a detailed description of
+    // that error.
+    std::string error_log;
   };
 
-  // Checks the Jacobian computed by a cost function.
+  // Call the cost function, compute alternative Jacobians using finite
+  // differencing and compare results. If local parameterizations are given,
+  // the Jacobians will be multiplied by the local parameterization Jacobians
+  // before performing the check, which effectively means that all errors along
+  // the null space of the local parameterization will be ignored.
+  // Returns false if the Jacobians don't match, the cost function return false,
+  // or if the cost function returns different residual when called with a
+  // Jacobian output argument vs. calling it without. Otherwise returns true.
   //
-  // 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.
+  // parameters: The parameter values at which to probe.
+  // relative_precision: A threshold for the relative difference between the
+  // Jacobians. If the Jacobians differ by more than this amount, then the
+  // probe fails.
+  // results: On return, the 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 std::vector<int32>& 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;
-  }
+  bool Probe(double const* const* parameters,
+             double relative_precision,
+             ProbeResults* results) const;
 
  private:
   CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(GradientChecker);
+
+  std::vector<const LocalParameterization*> local_parameterizations_;
+  const CostFunction* function_;
+  internal::scoped_ptr<CostFunction> finite_diff_cost_function_;
 };
 
 }  // namespace ceres
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index 5d24a8a..74b0799 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt
@@ -63,10 +63,12 @@
     dynamic_compressed_row_sparse_matrix.cc
     evaluator.cc
     file.cc
+    gradient_checker.cc
     gradient_checking_cost_function.cc
     gradient_problem.cc
     gradient_problem_solver.cc
     implicit_schur_complement.cc
+    is_close.cc
     iterative_schur_complement_solver.cc
     levenberg_marquardt_strategy.cc
     lapack.cc
@@ -309,6 +311,7 @@
   ceres_test(graph)
   ceres_test(graph_algorithms)
   ceres_test(householder_vector)
+  ceres_test(is_close)
   ceres_test(implicit_schur_complement)
   ceres_test(iterative_schur_complement_solver)
   ceres_test(jet)
diff --git a/internal/ceres/gradient_checker.cc b/internal/ceres/gradient_checker.cc
new file mode 100644
index 0000000..219027c
--- /dev/null
+++ b/internal/ceres/gradient_checker.cc
@@ -0,0 +1,277 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// 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.
+//
+// Authors: wjr@google.com (William Rucklidge),
+//          keir@google.com (Keir Mierle),
+//          dgossow@google.com (David Gossow)
+
+#include "ceres/gradient_checker.h"
+#include "ceres/is_close.h"
+
+#include <algorithm>
+#include <cmath>
+#include <numeric>
+#include <string>
+#include <vector>
+
+#include "ceres/is_close.h"
+#include "ceres/stringprintf.h"
+#include "ceres/types.h"
+
+namespace ceres {
+
+using internal::IsClose;
+using internal::StringAppendF;
+using internal::StringPrintf;
+using std::string;
+using std::vector;
+
+namespace {
+// Evaluate the cost function and transform the returned Jacobians to
+// the local space of the respective local parameterizations.
+bool EvaluateCostFunction(
+    const ceres::CostFunction* function,
+    double const* const * parameters,
+    const std::vector<const ceres::LocalParameterization*>&
+        local_parameterizations,
+    Vector* residuals,
+    std::vector<Matrix>* jacobians,
+    std::vector<Matrix>* local_jacobians) {
+  CHECK_NOTNULL(residuals);
+  CHECK_NOTNULL(jacobians);
+  CHECK_NOTNULL(local_jacobians);
+
+  const vector<int32>& block_sizes = function->parameter_block_sizes();
+  const int num_parameter_blocks = block_sizes.size();
+
+  // Allocate Jacobian matrices in local space.
+  local_jacobians->resize(num_parameter_blocks);
+  vector<double*> local_jacobian_data(num_parameter_blocks);
+  for (int i = 0; i < num_parameter_blocks; ++i) {
+    int block_size = block_sizes.at(i);
+    if (local_parameterizations.at(i) != NULL) {
+      block_size = local_parameterizations.at(i)->LocalSize();
+    }
+    local_jacobians->at(i).resize(function->num_residuals(), block_size);
+    local_jacobians->at(i).setZero();
+    local_jacobian_data.at(i) = local_jacobians->at(i).data();
+  }
+
+  // Allocate Jacobian matrices in global space.
+  jacobians->resize(num_parameter_blocks);
+  vector<double*> jacobian_data(num_parameter_blocks);
+  for (int i = 0; i < num_parameter_blocks; ++i) {
+    jacobians->at(i).resize(function->num_residuals(), block_sizes.at(i));
+    jacobians->at(i).setZero();
+    jacobian_data.at(i) = jacobians->at(i).data();
+  }
+
+  // Compute residuals & jacobians.
+  CHECK_NE(0, function->num_residuals());
+  residuals->resize(function->num_residuals());
+  residuals->setZero();
+  if (!function->Evaluate(parameters, residuals->data(),
+                          jacobian_data.data())) {
+    return false;
+  }
+
+  // Convert Jacobians from global to local space.
+  for (size_t i = 0; i < local_jacobians->size(); ++i) {
+    if (local_parameterizations.at(i) == NULL) {
+      local_jacobians->at(i) = jacobians->at(i);
+    } else {
+      int global_size = local_parameterizations.at(i)->GlobalSize();
+      int local_size = local_parameterizations.at(i)->LocalSize();
+      CHECK_EQ(jacobians->at(i).cols(), global_size);
+      Matrix global_J_local(global_size, local_size);
+      local_parameterizations.at(i)->ComputeJacobian(
+          parameters[i], global_J_local.data());
+      local_jacobians->at(i) = jacobians->at(i) * global_J_local;
+    }
+  }
+  return true;
+}
+} // namespace
+
+GradientChecker::GradientChecker(
+      const CostFunction* function,
+      const vector<const LocalParameterization*>* local_parameterizations,
+      const NumericDiffOptions& options) :
+        function_(function) {
+  CHECK_NOTNULL(function);
+  if (local_parameterizations != NULL) {
+    local_parameterizations_ = *local_parameterizations;
+  } else {
+    local_parameterizations_.resize(function->parameter_block_sizes().size(),
+                                    NULL);
+  }
+  DynamicNumericDiffCostFunction<CostFunction, CENTRAL>*
+      finite_diff_cost_function =
+      new DynamicNumericDiffCostFunction<CostFunction, CENTRAL>(
+          function, DO_NOT_TAKE_OWNERSHIP, options);
+  finite_diff_cost_function_.reset(finite_diff_cost_function);
+
+  const vector<int32>& parameter_block_sizes =
+      function->parameter_block_sizes();
+  const int num_parameter_blocks = parameter_block_sizes.size();
+  for (int i = 0; i < num_parameter_blocks; ++i) {
+    finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]);
+  }
+  finite_diff_cost_function->SetNumResiduals(function->num_residuals());
+}
+
+bool GradientChecker::Probe(double const* const * parameters,
+                            double relative_precision,
+                            ProbeResults* results_param) const {
+  int num_residuals = function_->num_residuals();
+
+  // Make sure that we have a place to store results, no matter if the user has
+  // provided an output argument.
+  ProbeResults* results;
+  ProbeResults results_local;
+  if (results_param != NULL) {
+    results = results_param;
+    results->residuals.resize(0);
+    results->jacobians.clear();
+    results->numeric_jacobians.clear();
+    results->local_jacobians.clear();
+    results->local_numeric_jacobians.clear();
+    results->error_log.clear();
+  } else {
+    results = &results_local;
+  }
+  results->maximum_relative_error = 0.0;
+  results->return_value = true;
+
+  // Evaluate the derivative using the user supplied code.
+  vector<Matrix>& jacobians = results->jacobians;
+  vector<Matrix>& local_jacobians = results->local_jacobians;
+  if (!EvaluateCostFunction(function_, parameters, local_parameterizations_,
+                       &results->residuals, &jacobians, &local_jacobians)) {
+    results->error_log = "Function evaluation with Jacobians failed.";
+    results->return_value = false;
+  }
+
+  // Evaluate the derivative using numeric derivatives.
+  vector<Matrix>& numeric_jacobians = results->numeric_jacobians;
+  vector<Matrix>& local_numeric_jacobians = results->local_numeric_jacobians;
+  Vector finite_diff_residuals;
+  if (!EvaluateCostFunction(finite_diff_cost_function_.get(), parameters,
+                            local_parameterizations_, &finite_diff_residuals,
+                            &numeric_jacobians, &local_numeric_jacobians)) {
+    results->error_log += "\nFunction evaluation with numerical "
+        "differentiation failed.";
+    results->return_value = false;
+  }
+
+  if (!results->return_value) {
+    return false;
+  }
+
+  for (int i = 0; i < num_residuals; ++i) {
+    if (!IsClose(
+        results->residuals[i],
+        finite_diff_residuals[i],
+        relative_precision,
+        NULL,
+        NULL)) {
+      results->error_log = "Function evaluation with and without Jacobians "
+          "resulted in different residuals.";
+      LOG(INFO) << results->residuals.transpose();
+      LOG(INFO) << finite_diff_residuals.transpose();
+      return false;
+    }
+  }
+
+  // See if any elements have relative error larger than the threshold.
+  int num_bad_jacobian_components = 0;
+  double& worst_relative_error = results->maximum_relative_error;
+  worst_relative_error = 0;
+
+  // Accumulate the error message for all the jacobians, since it won't get
+  // output if there are no bad jacobian components.
+  string error_log;
+  for (int k = 0; k < function_->parameter_block_sizes().size(); k++) {
+    StringAppendF(&error_log,
+                  "========== "
+                  "Jacobian for " "block %d: (%ld by %ld)) "
+                  "==========\n",
+                  k,
+                  static_cast<long>(local_jacobians[k].rows()),
+                  static_cast<long>(local_jacobians[k].cols()));
+    // The funny spacing creates appropriately aligned column headers.
+    error_log +=
+        " block  row  col        user dx/dy    num diff dx/dy         "
+        "abs error    relative error         parameter          residual\n";
+
+    for (int i = 0; i < local_jacobians[k].rows(); i++) {
+      for (int j = 0; j < local_jacobians[k].cols(); j++) {
+        double term_jacobian = local_jacobians[k](i, j);
+        double finite_jacobian = local_numeric_jacobians[k](i, j);
+        double relative_error, absolute_error;
+        bool bad_jacobian_entry =
+            !IsClose(term_jacobian,
+                     finite_jacobian,
+                     relative_precision,
+                     &relative_error,
+                     &absolute_error);
+        worst_relative_error = std::max(worst_relative_error, relative_error);
+
+        StringAppendF(&error_log,
+                      "%6d %4d %4d %17g %17g %17g %17g %17g %17g",
+                      k, i, j,
+                      term_jacobian, finite_jacobian,
+                      absolute_error, relative_error,
+                      parameters[k][j],
+                      results->residuals[i]);
+
+        if (bad_jacobian_entry) {
+          num_bad_jacobian_components++;
+          StringAppendF(
+              &error_log,
+              " ------ (%d,%d,%d) Relative error worse than %g",
+              k, i, j, relative_precision);
+        }
+        error_log += "\n";
+      }
+    }
+  }
+
+  // Since there were some bad errors, dump comprehensive debug info.
+  if (num_bad_jacobian_components) {
+    string header = StringPrintf("\nDetected %d bad Jacobian component(s). "
+        "Worst relative error was %g.\n",
+        num_bad_jacobian_components,
+        worst_relative_error);
+     results->error_log = header + "\n" + error_log;
+    return false;
+  }
+  return true;
+}
+
+}  // namespace ceres
diff --git a/internal/ceres/gradient_checker_test.cc b/internal/ceres/gradient_checker_test.cc
index 474ead5..d2ce6e3 100644
--- a/internal/ceres/gradient_checker_test.cc
+++ b/internal/ceres/gradient_checker_test.cc
@@ -38,6 +38,8 @@
 
 #include "ceres/cost_function.h"
 #include "ceres/random.h"
+#include "ceres/solver.h"
+#include "ceres/problem.h"
 #include "glog/logging.h"
 #include "gtest/gtest.h"
 
@@ -55,7 +57,7 @@
 // version, they are both block vectors, of course.
 class GoodTestTerm : public CostFunction {
  public:
-  GoodTestTerm(int arity, int const *dim) : arity_(arity) {
+  GoodTestTerm(int arity, int const *dim) : arity_(arity), return_value_(true) {
     // Make 'arity' random vectors.
     a_.resize(arity_);
     for (int j = 0; j < arity_; ++j) {
@@ -74,6 +76,9 @@
   bool Evaluate(double const* const* parameters,
                 double* residuals,
                 double** jacobians) const {
+    if (!return_value_) {
+      return false;
+    }
     // Compute a . x.
     double ax = 0;
     for (int j = 0; j < arity_; ++j) {
@@ -101,7 +106,12 @@
     return true;
   }
 
+  void SetReturnValue(bool return_value) {
+    return_value_ = return_value;
+  }
+
  private:
+  bool return_value_;
   int arity_;
   vector<vector<double> > a_;  // our vectors.
 };
@@ -159,37 +169,399 @@
   vector<vector<double> > a_;  // our vectors.
 };
 
+const double kTolerance = 1e-6;
+
+void CheckDimensions(
+    const GradientChecker::ProbeResults& results,
+    const std::vector<int>& parameter_sizes,
+    const std::vector<int>& local_parameter_sizes, int residual_size) {
+  CHECK_EQ(parameter_sizes.size(), local_parameter_sizes.size());
+  int num_parameters = parameter_sizes.size();
+  ASSERT_EQ(residual_size, results.residuals.size());
+  ASSERT_EQ(num_parameters, results.local_jacobians.size());
+  ASSERT_EQ(num_parameters, results.local_numeric_jacobians.size());
+  ASSERT_EQ(num_parameters, results.jacobians.size());
+  ASSERT_EQ(num_parameters, results.numeric_jacobians.size());
+  for (int i = 0; i < num_parameters; ++i) {
+    EXPECT_EQ(residual_size, results.local_jacobians.at(i).rows());
+    EXPECT_EQ(local_parameter_sizes[i], results.local_jacobians.at(i).cols());
+    EXPECT_EQ(residual_size, results.local_numeric_jacobians.at(i).rows());
+    EXPECT_EQ(local_parameter_sizes[i], results.local_numeric_jacobians.at(i).cols());
+    EXPECT_EQ(residual_size, results.jacobians.at(i).rows());
+    EXPECT_EQ(parameter_sizes[i], results.jacobians.at(i).cols());
+    EXPECT_EQ(residual_size, results.numeric_jacobians.at(i).rows());
+    EXPECT_EQ(parameter_sizes[i], results.numeric_jacobians.at(i).cols());
+  }
+}
+
 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 };
+  int const num_parameters = 3;
+  std::vector<int> parameter_sizes(3);
+  parameter_sizes[0] = 2;
+  parameter_sizes[1] = 3;
+  parameter_sizes[2] = 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) {
+  FixedArray<double*> parameters(num_parameters);
+  for (int j = 0; j < num_parameters; ++j) {
+    parameters[j] = new double[parameter_sizes[j]];
+    for (int u = 0; u < parameter_sizes[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));
+  NumericDiffOptions numeric_diff_options;
+  GradientChecker::ProbeResults results;
 
-  BadTestTerm bad_term(arity, dim);
-  typedef GradientChecker<BadTestTerm, 1, 2, 3, 4> BadTermGradientChecker;
-  EXPECT_FALSE(BadTermGradientChecker::Probe(
-      parameters.get(), 1e-6, &bad_term, NULL));
+  // Test that Probe returns true for correct Jacobians.
+  GoodTestTerm good_term(num_parameters, parameter_sizes.data());
+  GradientChecker good_gradient_checker(&good_term, NULL, numeric_diff_options);
+  EXPECT_TRUE(good_gradient_checker.Probe(parameters.get(), kTolerance, NULL));
+  EXPECT_TRUE(good_gradient_checker.Probe(parameters.get(), kTolerance,
+                                          &results))
+    << results.error_log;
 
-  for (int j = 0; j < arity; j++) {
+  // Check that results contain sensible data.
+  ASSERT_EQ(results.return_value, true);
+  ASSERT_EQ(results.residuals.size(), 1);
+  CheckDimensions(results, parameter_sizes, parameter_sizes, 1);
+  EXPECT_GE(results.maximum_relative_error, 0.0);
+  EXPECT_TRUE(results.error_log.empty());
+
+  // Test that if the cost function return false, Probe should return false.
+  good_term.SetReturnValue(false);
+  EXPECT_FALSE(good_gradient_checker.Probe(parameters.get(), kTolerance, NULL));
+  EXPECT_FALSE(good_gradient_checker.Probe(parameters.get(), kTolerance,
+                                           &results))
+    << results.error_log;
+
+  // Check that results contain sensible data.
+  ASSERT_EQ(results.return_value, false);
+  ASSERT_EQ(results.residuals.size(), 1);
+  CheckDimensions(results, parameter_sizes, parameter_sizes, 1);
+  for (int i = 0; i < num_parameters; ++i) {
+    EXPECT_EQ(results.local_jacobians.at(i).norm(), 0);
+    EXPECT_EQ(results.local_numeric_jacobians.at(i).norm(), 0);
+  }
+  EXPECT_EQ(results.maximum_relative_error, 0.0);
+  EXPECT_FALSE(results.error_log.empty());
+
+  // Test that Probe returns false for incorrect Jacobians.
+  BadTestTerm bad_term(num_parameters, parameter_sizes.data());
+  GradientChecker bad_gradient_checker(&bad_term, NULL, numeric_diff_options);
+  EXPECT_FALSE(bad_gradient_checker.Probe(parameters.get(), kTolerance, NULL));
+  EXPECT_FALSE(bad_gradient_checker.Probe(parameters.get(), kTolerance,
+                                          &results));
+
+  // Check that results contain sensible data.
+  ASSERT_EQ(results.return_value, true);
+  ASSERT_EQ(results.residuals.size(), 1);
+  CheckDimensions(results, parameter_sizes, parameter_sizes, 1);
+  EXPECT_GT(results.maximum_relative_error, kTolerance);
+  EXPECT_FALSE(results.error_log.empty());
+
+  // Setting a high threshold should make the test pass.
+  EXPECT_TRUE(bad_gradient_checker.Probe(parameters.get(), 1.0, &results));
+
+  // Check that results contain sensible data.
+  ASSERT_EQ(results.return_value, true);
+  ASSERT_EQ(results.residuals.size(), 1);
+  CheckDimensions(results, parameter_sizes, parameter_sizes, 1);
+  EXPECT_GT(results.maximum_relative_error, 0.0);
+  EXPECT_TRUE(results.error_log.empty());
+
+  for (int j = 0; j < num_parameters; j++) {
     delete[] parameters[j];
   }
 }
 
+
+/**
+ * Helper cost function that multiplies the parameters by the given jacobians
+ * and adds a constant offset.
+ */
+class LinearCostFunction : public CostFunction {
+ public:
+  explicit LinearCostFunction(const Vector& residuals_offset)
+      : residuals_offset_(residuals_offset) {
+    set_num_residuals(residuals_offset_.size());
+  }
+
+  virtual bool Evaluate(double const* const* parameter_ptrs, double* residuals_ptr,
+                        double** residual_J_params) const {
+    CHECK_GE(residual_J_params_.size(), 0.0);
+    VectorRef residuals(residuals_ptr, residual_J_params_[0].rows());
+    residuals = residuals_offset_;
+
+    for (size_t i = 0; i < residual_J_params_.size(); ++i) {
+      const Matrix& residual_J_param = residual_J_params_[i];
+      int parameter_size = residual_J_param.cols();
+      ConstVectorRef param(parameter_ptrs[i], parameter_size);
+
+      // Compute residual.
+      residuals += residual_J_param * param;
+
+      // Return Jacobian.
+      if (residual_J_params != NULL && residual_J_params[i] != NULL) {
+        Eigen::Map<Matrix> residual_J_param_out(residual_J_params[i],
+                                                       residual_J_param.rows(),
+                                                       residual_J_param.cols());
+        if (jacobian_offsets_.count(i) != 0) {
+          residual_J_param_out = residual_J_param + jacobian_offsets_.at(i);
+        } else {
+          residual_J_param_out = residual_J_param;
+        }
+      }
+    }
+    return true;
+  }
+
+  void AddParameter(const Matrix& residual_J_param) {
+    CHECK_EQ(num_residuals(), residual_J_param.rows());
+    residual_J_params_.push_back(residual_J_param);
+    mutable_parameter_block_sizes()->push_back(residual_J_param.cols());
+  }
+
+  /// Add offset to the given Jacobian before returning it from Evaluate(),
+  /// thus introducing an error in the comutation.
+  void SetJacobianOffset(size_t index, Matrix offset) {
+    CHECK_LT(index, residual_J_params_.size());
+    CHECK_EQ(residual_J_params_[index].rows(), offset.rows());
+    CHECK_EQ(residual_J_params_[index].cols(), offset.cols());
+    jacobian_offsets_[index] = offset;
+  }
+
+ private:
+  std::vector<Matrix> residual_J_params_;
+  std::map<int, Matrix> jacobian_offsets_;
+  Vector residuals_offset_;
+};
+
+/**
+ * Helper local parameterization that multiplies the delta vector by the given
+ * jacobian and adds it to the parameter.
+ */
+class MatrixParameterization : public LocalParameterization {
+ public:
+  virtual bool Plus(const double* x,
+                    const double* delta,
+                    double* x_plus_delta) const {
+    VectorRef(x_plus_delta, GlobalSize()) =
+        ConstVectorRef(x, GlobalSize()) +
+        (global_J_local * ConstVectorRef(delta, LocalSize()));
+    return true;
+  }
+
+  virtual bool ComputeJacobian(const double* /*x*/, double* jacobian) const {
+    MatrixRef(jacobian, GlobalSize(), LocalSize()) = global_J_local;
+    return true;
+  }
+
+  virtual int GlobalSize() const { return global_J_local.rows(); }
+  virtual int LocalSize() const { return global_J_local.cols(); }
+
+  Matrix global_J_local;
+};
+
+TEST(GradientChecker, TestCorrectnessWithLocalParameterizations) {
+  // Create cost function.
+  Eigen::Vector3d residual_offset(100.0, 200.0, 300.0);
+  LinearCostFunction cost_function(residual_offset);
+  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> j0;
+  j0.row(0) << 1.0, 2.0, 3.0;
+  j0.row(1) << 4.0, 5.0, 6.0;
+  j0.row(2) << 7.0, 8.0, 9.0;
+  Eigen::Matrix<double, 3, 2, Eigen::RowMajor> j1;
+  j1.row(0) << 10.0, 11.0;
+  j1.row(1) << 12.0, 13.0;
+  j1.row(2) << 14.0, 15.0;
+
+  Eigen::Vector3d param0(1.0, 2.0, 3.0);
+  Eigen::Vector2d param1(4.0, 5.0);
+
+  int const arity = 2;
+  const int dim[2] = {3, 2};
+
+  cost_function.AddParameter(j0);
+  cost_function.AddParameter(j1);
+
+  int const num_parameters = 2;
+  std::vector<int> parameter_sizes(2);
+  parameter_sizes[0] = 3;
+  parameter_sizes[1] = 2;
+  std::vector<int> local_parameter_sizes(2);
+  local_parameter_sizes[0] = 2;
+  local_parameter_sizes[1] = 2;
+
+  // Test cost function for correctness.
+  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> j1_out;
+  Eigen::Matrix<double, 3, 2, Eigen::RowMajor> j2_out;
+  Eigen::VectorXd residual(3);
+  std::vector<const double*> parameters(2);
+  parameters[0] = param0.data();
+  parameters[1] = param1.data();
+  std::vector<double*> jacobians(2);
+  jacobians[0] = j1_out.data();
+  jacobians[1] = j2_out.data();
+  cost_function.Evaluate(parameters.data(), residual.data(), jacobians.data());
+
+  Matrix residual_expected = residual_offset + j0 * param0 + j1 * param1;
+
+  EXPECT_TRUE(j1_out == j0);
+  EXPECT_TRUE(j2_out == j1);
+  EXPECT_TRUE(residual.isApprox(residual_expected, kTolerance));
+
+  // Create local parameterization.
+  Eigen::Matrix<double, 3, 2, Eigen::RowMajor> global_J_local;
+  global_J_local.row(0) << 1.5, 2.5;
+  global_J_local.row(1) << 3.5, 4.5;
+  global_J_local.row(2) << 5.5, 6.5;
+
+  MatrixParameterization parameterization;
+  parameterization.global_J_local = global_J_local;
+
+  // Test local parameterization for correctness.
+  Eigen::Vector3d x(7.0, 8.0, 9.0);
+  Eigen::Vector2d delta(10.0, 11.0);
+
+  Eigen::Matrix<double, 3, 2, Eigen::RowMajor> global_J_local_out;
+  parameterization.ComputeJacobian(x.data(), global_J_local_out.data());
+  EXPECT_TRUE(global_J_local_out == global_J_local);
+
+  Eigen::Vector3d x_plus_delta;
+  parameterization.Plus(x.data(), delta.data(), x_plus_delta.data());
+  Eigen::Vector3d x_plus_delta_expected = x + (global_J_local * delta);
+  EXPECT_TRUE(x_plus_delta.isApprox(x_plus_delta_expected, kTolerance));
+
+  // Now test GradientChecker.
+  std::vector<const LocalParameterization*> parameterizations(2);
+  parameterizations[0] = &parameterization;
+  parameterizations[1] = NULL;
+  NumericDiffOptions numeric_diff_options;
+  GradientChecker::ProbeResults results;
+  GradientChecker gradient_checker(&cost_function, &parameterizations,
+                                   numeric_diff_options);
+
+  Problem::Options problem_options;
+  problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP;
+  problem_options.local_parameterization_ownership = DO_NOT_TAKE_OWNERSHIP;
+  Problem problem(problem_options);
+  Eigen::Vector3d param0_solver;
+  Eigen::Vector2d param1_solver;
+  problem.AddParameterBlock(param0_solver.data(), 3, &parameterization);
+  problem.AddParameterBlock(param1_solver.data(), 2);
+  problem.AddResidualBlock(&cost_function, NULL, param0_solver.data(),
+                           param1_solver.data());
+  Solver::Options solver_options;
+  solver_options.check_gradients = true;
+  solver_options.initial_trust_region_radius = 1e10;
+  Solver solver;
+  Solver::Summary summary;
+
+  // First test case: everything is correct.
+  EXPECT_TRUE(gradient_checker.Probe(parameters.data(), kTolerance, NULL));
+  EXPECT_TRUE(gradient_checker.Probe(parameters.data(), kTolerance, &results))
+    << results.error_log;
+
+  // Check that results contain correct data.
+  ASSERT_EQ(results.return_value, true);
+  ASSERT_TRUE(results.residuals == residual);
+  CheckDimensions(results, parameter_sizes, local_parameter_sizes, 3);
+  EXPECT_TRUE(results.local_jacobians.at(0) == j0 * global_J_local);
+  EXPECT_TRUE(results.local_jacobians.at(1) == j1);
+  EXPECT_TRUE(results.local_numeric_jacobians.at(0).isApprox(
+      j0 * global_J_local, kTolerance));
+  EXPECT_TRUE(results.local_numeric_jacobians.at(1).isApprox(
+      j1, kTolerance));
+  EXPECT_TRUE(results.jacobians.at(0) == j0);
+  EXPECT_TRUE(results.jacobians.at(1) == j1);
+  EXPECT_TRUE(results.numeric_jacobians.at(0).isApprox(
+      j0, kTolerance));
+  EXPECT_TRUE(results.numeric_jacobians.at(1).isApprox(
+      j1, kTolerance));
+  EXPECT_GE(results.maximum_relative_error, 0.0);
+  EXPECT_TRUE(results.error_log.empty());
+
+  // Test interaction with the 'check_gradients' option in Solver.
+  param0_solver = param0;
+  param1_solver = param1;
+  solver.Solve(solver_options, &problem, &summary);
+  EXPECT_EQ(CONVERGENCE, summary.termination_type);
+  EXPECT_LE(summary.final_cost, 1e-12);
+
+  // Second test case: Mess up reported derivatives with respect to 3rd
+  // component of 1st parameter. Check should fail.
+  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> j0_offset;
+  j0_offset.setZero();
+  j0_offset.col(2).setConstant(0.001);
+  cost_function.SetJacobianOffset(0, j0_offset);
+  EXPECT_FALSE(gradient_checker.Probe(parameters.data(), kTolerance, NULL));
+  EXPECT_FALSE(gradient_checker.Probe(parameters.data(), kTolerance, &results))
+    << results.error_log;
+
+  // Check that results contain correct data.
+  ASSERT_EQ(results.return_value, true);
+  ASSERT_TRUE(results.residuals == residual);
+  CheckDimensions(results, parameter_sizes, local_parameter_sizes, 3);
+  ASSERT_EQ(results.local_jacobians.size(), 2);
+  ASSERT_EQ(results.local_numeric_jacobians.size(), 2);
+  EXPECT_TRUE(results.local_jacobians.at(0) == (j0 + j0_offset) * global_J_local);
+  EXPECT_TRUE(results.local_jacobians.at(1) == j1);
+  EXPECT_TRUE(
+      results.local_numeric_jacobians.at(0).isApprox(j0 * global_J_local,
+                                                     kTolerance));
+  EXPECT_TRUE(results.local_numeric_jacobians.at(1).isApprox(j1, kTolerance));
+  EXPECT_TRUE(results.jacobians.at(0) == j0 + j0_offset);
+  EXPECT_TRUE(results.jacobians.at(1) == j1);
+  EXPECT_TRUE(results.numeric_jacobians.at(0).isApprox(j0, kTolerance));
+  EXPECT_TRUE(results.numeric_jacobians.at(1).isApprox(j1, kTolerance));
+  EXPECT_GT(results.maximum_relative_error, 0.0);
+  EXPECT_FALSE(results.error_log.empty());
+
+  // Test interaction with the 'check_gradients' option in Solver.
+  param0_solver = param0;
+  param1_solver = param1;
+  solver.Solve(solver_options, &problem, &summary);
+  EXPECT_EQ(FAILURE, summary.termination_type);
+
+  // Now, zero out the local parameterization Jacobian of the 1st parameter
+  // with respect to the 3rd component. This makes the combination of
+  // cost function and local parameterization return correct values again.
+  parameterization.global_J_local.row(2).setZero();
+
+  // Verify that the gradient checker does not treat this as an error.
+  EXPECT_TRUE(gradient_checker.Probe(parameters.data(), kTolerance, &results))
+    << results.error_log;
+
+  // Check that results contain correct data.
+  ASSERT_EQ(results.return_value, true);
+  ASSERT_TRUE(results.residuals == residual);
+  CheckDimensions(results, parameter_sizes, local_parameter_sizes, 3);
+  ASSERT_EQ(results.local_jacobians.size(), 2);
+  ASSERT_EQ(results.local_numeric_jacobians.size(), 2);
+  EXPECT_TRUE(results.local_jacobians.at(0) ==
+      (j0 + j0_offset) * parameterization.global_J_local);
+  EXPECT_TRUE(results.local_jacobians.at(1) == j1);
+  EXPECT_TRUE(results.local_numeric_jacobians.at(0).isApprox(
+      j0 * parameterization.global_J_local, kTolerance));
+  EXPECT_TRUE(results.local_numeric_jacobians.at(1).isApprox(j1, kTolerance));
+  EXPECT_TRUE(results.jacobians.at(0) == j0 + j0_offset);
+  EXPECT_TRUE(results.jacobians.at(1) == j1);
+  EXPECT_TRUE(results.numeric_jacobians.at(0).isApprox(j0, kTolerance));
+  EXPECT_TRUE(results.numeric_jacobians.at(1).isApprox(j1, kTolerance));
+  EXPECT_GE(results.maximum_relative_error, 0.0);
+  EXPECT_TRUE(results.error_log.empty());
+
+  // Test interaction with the 'check_gradients' option in Solver.
+  param0_solver = param0;
+  param1_solver = param1;
+  solver.Solve(solver_options, &problem, &summary);
+  EXPECT_EQ(CONVERGENCE, summary.termination_type);
+  EXPECT_LE(summary.final_cost, 1e-12);
+}
+
 }  // namespace internal
 }  // namespace ceres
diff --git a/internal/ceres/gradient_checking_cost_function.cc b/internal/ceres/gradient_checking_cost_function.cc
index 580fd26..6f0c59b 100644
--- a/internal/ceres/gradient_checking_cost_function.cc
+++ b/internal/ceres/gradient_checking_cost_function.cc
@@ -26,7 +26,8 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 //
-// Author: keir@google.com (Keir Mierle)
+// Authors: keir@google.com (Keir Mierle),
+//          dgossow@google.com (David Gossow)
 
 #include "ceres/gradient_checking_cost_function.h"
 
@@ -36,7 +37,7 @@
 #include <string>
 #include <vector>
 
-#include "ceres/cost_function.h"
+#include "ceres/gradient_checker.h"
 #include "ceres/internal/eigen.h"
 #include "ceres/internal/scoped_ptr.h"
 #include "ceres/parameter_block.h"
@@ -59,55 +60,25 @@
 
 namespace {
 
-// True if x and y have an absolute relative difference less than
-// relative_precision and false otherwise. Stores the relative and absolute
-// difference in relative/absolute_error if non-NULL.
-bool IsClose(double x, double y, double relative_precision,
-             double *relative_error,
-             double *absolute_error) {
-  double local_absolute_error;
-  double local_relative_error;
-  if (!absolute_error) {
-    absolute_error = &local_absolute_error;
-  }
-  if (!relative_error) {
-    relative_error = &local_relative_error;
-  }
-  *absolute_error = abs(x - y);
-  *relative_error = *absolute_error / max(abs(x), abs(y));
-  if (x == 0 || y == 0) {
-    // If x or y is exactly zero, then relative difference doesn't have any
-    // meaning. Take the absolute difference instead.
-    *relative_error = *absolute_error;
-  }
-  return abs(*relative_error) < abs(relative_precision);
-}
-
 class GradientCheckingCostFunction : public CostFunction {
  public:
-  GradientCheckingCostFunction(const CostFunction* function,
-                               const NumericDiffOptions& options,
-                               double relative_precision,
-                               const string& extra_info)
+  GradientCheckingCostFunction(
+      const CostFunction* function,
+      const std::vector<const LocalParameterization*>* local_parameterizations,
+      const NumericDiffOptions& options,
+      double relative_precision,
+      const string& extra_info,
+      GradientCheckingIterationCallback* callback)
       : function_(function),
+        gradient_checker_(function, local_parameterizations, options),
         relative_precision_(relative_precision),
-        extra_info_(extra_info) {
-    DynamicNumericDiffCostFunction<CostFunction, CENTRAL>*
-        finite_diff_cost_function =
-        new DynamicNumericDiffCostFunction<CostFunction, CENTRAL>(
-            function,
-            DO_NOT_TAKE_OWNERSHIP,
-            options);
-
+        extra_info_(extra_info),
+        callback_(callback) {
+    CHECK_NOTNULL(callback_);
     const vector<int32>& parameter_block_sizes =
         function->parameter_block_sizes();
-    for (int i = 0; i < parameter_block_sizes.size(); ++i) {
-      finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]);
-    }
     *mutable_parameter_block_sizes() = parameter_block_sizes;
     set_num_residuals(function->num_residuals());
-    finite_diff_cost_function->SetNumResiduals(num_residuals());
-    finite_diff_cost_function_.reset(finite_diff_cost_function);
   }
 
   virtual ~GradientCheckingCostFunction() { }
@@ -120,133 +91,90 @@
       return function_->Evaluate(parameters, residuals, NULL);
     }
 
-    int num_residuals = function_->num_residuals();
+    GradientChecker::ProbeResults results;
+    bool okay = gradient_checker_.Probe(parameters,
+                                        relative_precision_,
+                                        &results);
 
-    // Make space for the jacobians of the two methods.
-    const vector<int32>& block_sizes = function_->parameter_block_sizes();
-    vector<Matrix> term_jacobians(block_sizes.size());
-    vector<Matrix> finite_difference_jacobians(block_sizes.size());
-    vector<double*> term_jacobian_pointers(block_sizes.size());
-    vector<double*> finite_difference_jacobian_pointers(block_sizes.size());
-    for (int i = 0; i < block_sizes.size(); i++) {
-      term_jacobians[i].resize(num_residuals, block_sizes[i]);
-      term_jacobian_pointers[i] = term_jacobians[i].data();
-      finite_difference_jacobians[i].resize(num_residuals, block_sizes[i]);
-      finite_difference_jacobian_pointers[i] =
-          finite_difference_jacobians[i].data();
-    }
-
-    // Evaluate the derivative using the user supplied code.
-    if (!function_->Evaluate(parameters,
-                             residuals,
-                             &term_jacobian_pointers[0])) {
-      LOG(WARNING) << "Function evaluation failed.";
+    // If the cost function returned false, there's nothing we can say about
+    // the gradients.
+    if (results.return_value == false) {
       return false;
     }
 
-    // Evaluate the derivative using numeric derivatives.
-    finite_diff_cost_function_->Evaluate(
-        parameters,
-        residuals,
-        &finite_difference_jacobian_pointers[0]);
+    // Copy the residuals.
+    const int num_residuals = function_->num_residuals();
+    MatrixRef(residuals, num_residuals, 1) = results.residuals;
 
-    // See if any elements have relative error larger than the threshold.
-    int num_bad_jacobian_components = 0;
-    double worst_relative_error = 0;
-
-    // Accumulate the error message for all the jacobians, since it won't get
-    // output if there are no bad jacobian components.
-    string m;
+    // Copy the original jacobian blocks into the jacobians array.
+    const vector<int32>& block_sizes = function_->parameter_block_sizes();
     for (int k = 0; k < block_sizes.size(); k++) {
-      // Copy the original jacobian blocks into the jacobians array.
       if (jacobians[k] != NULL) {
         MatrixRef(jacobians[k],
-                  term_jacobians[k].rows(),
-                  term_jacobians[k].cols()) = term_jacobians[k];
-      }
-
-      StringAppendF(&m,
-                    "========== "
-                    "Jacobian for " "block %d: (%ld by %ld)) "
-                    "==========\n",
-                    k,
-                    static_cast<long>(term_jacobians[k].rows()),
-                    static_cast<long>(term_jacobians[k].cols()));
-      // The funny spacing creates appropriately aligned column headers.
-      m += " block  row  col        user dx/dy    num diff dx/dy         "
-           "abs error    relative error         parameter          residual\n";
-
-      for (int i = 0; i < term_jacobians[k].rows(); i++) {
-        for (int j = 0; j < term_jacobians[k].cols(); j++) {
-          double term_jacobian = term_jacobians[k](i, j);
-          double finite_jacobian = finite_difference_jacobians[k](i, j);
-          double relative_error, absolute_error;
-          bool bad_jacobian_entry =
-              !IsClose(term_jacobian,
-                       finite_jacobian,
-                       relative_precision_,
-                       &relative_error,
-                       &absolute_error);
-          worst_relative_error = max(worst_relative_error, relative_error);
-
-          StringAppendF(&m, "%6d %4d %4d %17g %17g %17g %17g %17g %17g",
-                        k, i, j,
-                        term_jacobian, finite_jacobian,
-                        absolute_error, relative_error,
-                        parameters[k][j],
-                        residuals[i]);
-
-          if (bad_jacobian_entry) {
-            num_bad_jacobian_components++;
-            StringAppendF(
-                &m, " ------ (%d,%d,%d) Relative error worse than %g",
-                k, i, j, relative_precision_);
-          }
-          m += "\n";
-        }
+                  results.jacobians[k].rows(),
+                  results.jacobians[k].cols()) = results.jacobians[k];
       }
     }
 
-    // Since there were some bad errors, dump comprehensive debug info.
-    if (num_bad_jacobian_components) {
-      string header = StringPrintf("Detected %d bad jacobian component(s). "
-                                   "Worst relative error was %g.\n",
-                                   num_bad_jacobian_components,
-                                   worst_relative_error);
-      if (!extra_info_.empty()) {
-        header += "Extra info for this residual: " + extra_info_ + "\n";
-      }
-      LOG(WARNING) << "\n" << header << m;
+    if (!okay) {
+      std::string error_log = "Gradient Error detected!\nExtra info for "
+          "this residual: " + extra_info_ + "\n" + results.error_log;
+      callback_->SetGradientErrorDetected(error_log);
     }
     return true;
   }
 
  private:
   const CostFunction* function_;
-  internal::scoped_ptr<CostFunction> finite_diff_cost_function_;
+  GradientChecker gradient_checker_;
   double relative_precision_;
   string extra_info_;
+  GradientCheckingIterationCallback* callback_;
 };
 
 }  // namespace
 
-CostFunction *CreateGradientCheckingCostFunction(
-    const CostFunction *cost_function,
+GradientCheckingIterationCallback::GradientCheckingIterationCallback()
+    : gradient_error_detected_(false) {
+}
+
+CallbackReturnType GradientCheckingIterationCallback::operator()(
+    const IterationSummary& summary) {
+  if (gradient_error_detected_) {
+    LOG(ERROR)<< "Gradient error detected. Terminating solver.";
+    return SOLVER_ABORT;
+  }
+  return SOLVER_CONTINUE;
+}
+void GradientCheckingIterationCallback::SetGradientErrorDetected(
+    std::string& error_log) {
+  gradient_error_detected_ = true;
+  error_log_ += "\n" + error_log;
+}
+
+CostFunction* CreateGradientCheckingCostFunction(
+    const CostFunction* cost_function,
+    const std::vector<const LocalParameterization*>* local_parameterizations,
     double relative_step_size,
     double relative_precision,
-    const string& extra_info) {
+    const std::string& extra_info,
+    GradientCheckingIterationCallback* callback) {
   NumericDiffOptions numeric_diff_options;
   numeric_diff_options.relative_step_size = relative_step_size;
 
   return new GradientCheckingCostFunction(cost_function,
+                                          local_parameterizations,
                                           numeric_diff_options,
-                                          relative_precision,
-                                          extra_info);
+                                          relative_precision, extra_info,
+                                          callback);
 }
 
-ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl,
-                                               double relative_step_size,
-                                               double relative_precision) {
+ProblemImpl* CreateGradientCheckingProblemImpl(
+    ProblemImpl* problem_impl,
+    double relative_step_size,
+    double relative_precision,
+    GradientCheckingIterationCallback* callback) {
+  CHECK_NOTNULL(callback);
   // We create new CostFunctions by wrapping the original CostFunction
   // in a gradient checking CostFunction. So its okay for the
   // ProblemImpl to take ownership of it and destroy it. The
@@ -260,6 +188,9 @@
   gradient_checking_problem_options.local_parameterization_ownership =
       DO_NOT_TAKE_OWNERSHIP;
 
+  NumericDiffOptions numeric_diff_options;
+  numeric_diff_options.relative_step_size = relative_step_size;
+
   ProblemImpl* gradient_checking_problem_impl = new ProblemImpl(
       gradient_checking_problem_options);
 
@@ -294,19 +225,26 @@
     string extra_info = StringPrintf(
         "Residual block id %d; depends on parameters [", i);
     vector<double*> parameter_blocks;
+    vector<const LocalParameterization*> local_parameterizations;
+    parameter_blocks.reserve(residual_block->NumParameterBlocks());
+    local_parameterizations.reserve(residual_block->NumParameterBlocks());
     for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) {
       ParameterBlock* parameter_block = residual_block->parameter_blocks()[j];
       parameter_blocks.push_back(parameter_block->mutable_user_state());
       StringAppendF(&extra_info, "%p", parameter_block->mutable_user_state());
       extra_info += (j < residual_block->NumParameterBlocks() - 1) ? ", " : "]";
+      local_parameterizations.push_back(problem_impl->GetParameterization(
+          parameter_block->mutable_user_state()));
     }
 
     // Wrap the original CostFunction in a GradientCheckingCostFunction.
     CostFunction* gradient_checking_cost_function =
-        CreateGradientCheckingCostFunction(residual_block->cost_function(),
-                                           relative_step_size,
-                                           relative_precision,
-                                           extra_info);
+        new GradientCheckingCostFunction(residual_block->cost_function(),
+                                         &local_parameterizations,
+                                         numeric_diff_options,
+                                         relative_precision,
+                                         extra_info,
+                                         callback);
 
     // The const_cast is necessary because
     // ProblemImpl::AddResidualBlock can potentially take ownership of
diff --git a/internal/ceres/gradient_checking_cost_function.h b/internal/ceres/gradient_checking_cost_function.h
index cf92cb7..d8bbfed 100644
--- a/internal/ceres/gradient_checking_cost_function.h
+++ b/internal/ceres/gradient_checking_cost_function.h
@@ -26,7 +26,8 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 //
-// Author: keir@google.com (Keir Mierle)
+// Authors: keir@google.com (Keir Mierle),
+//          dgossow@google.com (David Gossow)
 
 #ifndef CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_
 #define CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_
@@ -34,50 +35,72 @@
 #include <string>
 
 #include "ceres/cost_function.h"
+#include "ceres/iteration_callback.h"
+#include "ceres/local_parameterization.h"
 
 namespace ceres {
 namespace internal {
 
 class ProblemImpl;
 
-// Creates a CostFunction that checks the jacobians that cost_function computes
-// with finite differences. Bad results are logged; required precision is
-// controlled by relative_precision and the numeric differentiation step size is
-// controlled with relative_step_size. See solver.h for a better explanation of
-// relative_step_size. Caller owns result.
-//
-// The condition enforced is that
-//
-//    (J_actual(i, j) - J_numeric(i, j))
-//   ------------------------------------  <  relative_precision
-//   max(J_actual(i, j), J_numeric(i, j))
-//
-// where J_actual(i, j) is the jacobian as computed by the supplied cost
-// function (by the user) and J_numeric is the jacobian as computed by finite
-// differences.
-//
-// Note: This is quite inefficient and is intended only for debugging.
+// Callback that collects information about gradient checking errors, and
+// will abort the solve as soon as an error occurs.
+class GradientCheckingIterationCallback : public IterationCallback {
+ public:
+  GradientCheckingIterationCallback();
+
+  // Will return SOLVER_CONTINUE until a gradient error has been detected,
+  // then return SOLVER_ABORT.
+  virtual CallbackReturnType operator()(const IterationSummary& summary);
+
+  // Notify this that a gradient error has occured.
+  void SetGradientErrorDetected(std::string& error_log);
+
+  bool gradient_error_detected() const { return gradient_error_detected_; }
+  const std::string& error_log() const { return error_log_; }
+ private:
+  bool gradient_error_detected_;
+  std::string error_log_;
+};
+
+// Creates a CostFunction that checks the Jacobians that cost_function computes
+// with finite differences. This API is only intended for unit tests that intend
+// to  check the functionality of the GradientCheckingCostFunction
+// implementation directly.
 CostFunction* CreateGradientCheckingCostFunction(
     const CostFunction* cost_function,
+    const std::vector<const LocalParameterization*>* local_parameterizations,
     double relative_step_size,
     double relative_precision,
-    const std::string& extra_info);
+    const std::string& extra_info,
+    GradientCheckingIterationCallback* callback);
 
-// Create a new ProblemImpl object from the input problem_impl, where
-// each CostFunctions in problem_impl are wrapped inside a
-// GradientCheckingCostFunctions. This gives us a ProblemImpl object
-// which checks its derivatives against estimates from numeric
-// differentiation everytime a ResidualBlock is evaluated.
+// Create a new ProblemImpl object from the input problem_impl, where all
+// cost functions are wrapped so that each time their Evaluate method is called,
+// an additional check is performed that compares the Jacobians computed by
+// the original cost function with alternative Jacobians computed using
+// numerical differentiation. If local parameterizations are given for any
+// parameters, the Jacobians will be compared in the local space instead of the
+// ambient space. For details on the gradient checking procedure, see the
+// documentation of the GradientChecker class. If an error is detected in any
+// iteration, the respective cost function will notify the
+// GradientCheckingIterationCallback.
+//
+// The caller owns the returned ProblemImpl object.
+//
+// Note: This is quite inefficient and is intended only for debugging.
 //
 // relative_step_size and relative_precision are parameters to control
 // the numeric differentiation and the relative tolerance between the
 // jacobian computed by the CostFunctions in problem_impl and
-// jacobians obtained by numerically differentiating them. For more
-// details see the documentation for
-// CreateGradientCheckingCostFunction above.
-ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl,
-                                               double relative_step_size,
-                                               double relative_precision);
+// jacobians obtained by numerically differentiating them. See the
+// documentation of 'numeric_derivative_relative_step_size' in solver.h for a
+// better explanation.
+ProblemImpl* CreateGradientCheckingProblemImpl(
+    ProblemImpl* problem_impl,
+    double relative_step_size,
+    double relative_precision,
+    GradientCheckingIterationCallback* callback);
 
 }  // namespace internal
 }  // namespace ceres
diff --git a/internal/ceres/gradient_checking_cost_function_test.cc b/internal/ceres/gradient_checking_cost_function_test.cc
index 54c6166..e7238c6 100644
--- a/internal/ceres/gradient_checking_cost_function_test.cc
+++ b/internal/ceres/gradient_checking_cost_function_test.cc
@@ -162,11 +162,12 @@
   const double kRelativePrecision = 1e-4;
 
   TestTerm<-1, -1> term(arity, dim);
+  GradientCheckingIterationCallback callback;
   scoped_ptr<CostFunction> gradient_checking_cost_function(
-      CreateGradientCheckingCostFunction(&term,
+      CreateGradientCheckingCostFunction(&term, NULL,
                                          kRelativeStepSize,
                                          kRelativePrecision,
-                                         "Ignored."));
+                                         "Ignored.", &callback));
   term.Evaluate(&parameters[0],
                 &original_residual,
                 &original_jacobians[0]);
@@ -218,39 +219,35 @@
   LOG(INFO) << "Bad gradient";
   {
     TestTerm<1, 2> term(arity, dim);
+    GradientCheckingIterationCallback callback;
     scoped_ptr<CostFunction> gradient_checking_cost_function(
-        CreateGradientCheckingCostFunction(&term,
+        CreateGradientCheckingCostFunction(&term, NULL,
                                            kRelativeStepSize,
                                            kRelativePrecision,
-                                           "Fuzzy bananas"));
-
-    ScopedMockLog log;
-    EXPECT_CALL(log, Log(_, _, _)).Times(AnyNumber());
-    EXPECT_CALL(log, Log(WARNING, _,
-                         AllOf(HasSubstr("(1,0,2) Relative error worse than"),
-                               HasSubstr("Fuzzy bananas"))));
-
-    gradient_checking_cost_function->Evaluate(&parameters[0],
-                                              &residual,
-                                              &jacobians[0]);
+                                           "Fuzzy banana", &callback));
+    EXPECT_TRUE(
+        gradient_checking_cost_function->Evaluate(&parameters[0], &residual,
+                                                  &jacobians[0]));
+    EXPECT_TRUE(callback.gradient_error_detected());
+    EXPECT_TRUE(callback.error_log().find("Fuzzy banana") != std::string::npos);
+    EXPECT_TRUE(callback.error_log().find("(1,0,2) Relative error worse than")
+                != std::string::npos);
   }
 
   // The gradient is correct, so no errors are reported.
   LOG(INFO) << "Good gradient";
   {
     TestTerm<-1, -1> term(arity, dim);
+    GradientCheckingIterationCallback callback;
     scoped_ptr<CostFunction> gradient_checking_cost_function(
-        CreateGradientCheckingCostFunction(&term,
+        CreateGradientCheckingCostFunction(&term, NULL,
                                            kRelativeStepSize,
                                            kRelativePrecision,
-                                           "Ignored."));
-
-    ScopedMockLog log;
-    EXPECT_CALL(log, Log(_, _, _)).Times(0);
-
-    gradient_checking_cost_function->Evaluate(&parameters[0],
-                                              &residual,
-                                              &jacobians[0]);
+                                           "Fuzzy banana", &callback));
+    EXPECT_TRUE(
+        gradient_checking_cost_function->Evaluate(&parameters[0], &residual,
+                                                  &jacobians[0]));
+    EXPECT_FALSE(callback.gradient_error_detected());
   }
 
   for (int j = 0; j < arity; j++) {
@@ -362,8 +359,9 @@
   problem_impl.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4),
                                 NULL, z, x, y);
 
+  GradientCheckingIterationCallback callback;
   scoped_ptr<ProblemImpl> gradient_checking_problem_impl(
-      CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0));
+      CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0, &callback));
 
   // The dimensions of the two problems match.
   EXPECT_EQ(problem_impl.NumParameterBlocks(),
@@ -383,7 +381,7 @@
   // Since we added the ParameterBlocks and ResidualBlocks explicitly,
   // they should be in the same order in the two programs. It is
   // possible that may change due to implementation changes to
-  // Program. This is not exepected to be the case and writing code to
+  // Program. This is not expected to be the case and writing code to
   // anticipate that possibility not worth the extra complexity in
   // this test.
   for (int i = 0; i < program.parameter_blocks().size(); ++i) {
diff --git a/internal/ceres/is_close.cc b/internal/ceres/is_close.cc
new file mode 100644
index 0000000..a91a174
--- /dev/null
+++ b/internal/ceres/is_close.cc
@@ -0,0 +1,59 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// 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.
+//
+// Authors: keir@google.com (Keir Mierle), dgossow@google.com (David Gossow)
+
+#include "ceres/is_close.h"
+
+#include <algorithm>
+#include <cmath>
+
+namespace ceres {
+namespace internal {
+bool IsClose(double x, double y, double relative_precision,
+             double *relative_error,
+             double *absolute_error) {
+  double local_absolute_error;
+  double local_relative_error;
+  if (!absolute_error) {
+    absolute_error = &local_absolute_error;
+  }
+  if (!relative_error) {
+    relative_error = &local_relative_error;
+  }
+  *absolute_error = std::fabs(x - y);
+  *relative_error = *absolute_error / std::max(std::fabs(x), std::fabs(y));
+  if (x == 0 || y == 0) {
+    // If x or y is exactly zero, then relative difference doesn't have any
+    // meaning. Take the absolute difference instead.
+    *relative_error = *absolute_error;
+  }
+  return *relative_error < std::fabs(relative_precision);
+}
+}  // namespace internal
+}  // namespace ceres
diff --git a/internal/ceres/is_close.h b/internal/ceres/is_close.h
new file mode 100644
index 0000000..7789448
--- /dev/null
+++ b/internal/ceres/is_close.h
@@ -0,0 +1,51 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// 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.
+//
+// Authors: keir@google.com (Keir Mierle), dgossow@google.com (David Gossow)
+//
+// Utility routine for comparing two values.
+
+#ifndef CERES_INTERNAL_IS_CLOSE_H_
+#define CERES_INTERNAL_IS_CLOSE_H_
+
+namespace ceres {
+namespace internal {
+// Returns true if x and y have a relative (unsigned) difference less than
+// relative_precision and false otherwise. Stores the relative and absolute
+// difference in relative/absolute_error if non-NULL. If one of the two values
+// is exactly zero, the absolute difference will be compared, and relative_error
+// will be set to the absolute difference.
+bool IsClose(double x,
+             double y,
+             double relative_precision,
+             double *relative_error,
+             double *absolute_error);
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_INTERNAL_IS_CLOSE_H_
diff --git a/internal/ceres/is_close_test.cc b/internal/ceres/is_close_test.cc
new file mode 100644
index 0000000..8f7aaba
--- /dev/null
+++ b/internal/ceres/is_close_test.cc
@@ -0,0 +1,177 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// 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: dgossow@google.com (David Gossow)
+//
+// This file contains tests for the IsClose function.
+
+#include "ceres/is_close.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+const double kTolerance = 1e-9;
+
+TEST(IsClose, BothParametersPositive) {
+  double relative_error = -1;
+  double absolute_error = -1;
+
+  // Test cases where both values are positive.
+  EXPECT_TRUE(IsClose(9.9, 10.0, 0.011, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 0.01, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.1, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_TRUE(IsClose(10.0, 9.9, 0.011, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 0.01, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.1, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+
+  EXPECT_FALSE(IsClose(9.9, 10.0, 0.009, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 0.01, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.1, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_FALSE(IsClose(10.0, 9.9, 0.009, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 0.01, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.1, kTolerance);
+}
+
+TEST(IsClose, BothParametersNegative) {
+  double relative_error = -1;
+  double absolute_error = -1;
+
+  // Test cases where both values are negative.
+  EXPECT_TRUE(IsClose(-9.9, -10.0, 0.011, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 0.01, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.1, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_TRUE(IsClose(-10.0, -9.9, 0.011, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 0.01, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.1, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+
+  EXPECT_FALSE(IsClose(-9.9, -10.0, 0.009, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 0.01, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.1, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_FALSE(IsClose(-10.0, -9.9, 0.009, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 0.01, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.1, kTolerance);
+}
+
+TEST(IsClose, ParametersHaveMixedSigns) {
+  double relative_error = -1;
+  double absolute_error = -1;
+
+  // Test cases with mixed signs.
+  EXPECT_FALSE(IsClose(-0.1, 0.1, 1.99, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 2.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.2, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_TRUE(IsClose(-0.1, 0.1, 2.01, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 2.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.2, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_FALSE(IsClose(0.1, -0.1, 1.99, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 2.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.2, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_TRUE(IsClose(0.1, -0.1, 2.01, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 2.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.2, kTolerance);
+}
+
+TEST(IsClose, OneParameterZero) {
+  double relative_error = -1;
+  double absolute_error = -1;
+
+  // Test cases where one of the values is zero.
+  EXPECT_TRUE(IsClose(0.0, 10.0, 10.1, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 10.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 10.0, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_TRUE(IsClose(10.0, 0.0, 10.1, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 10.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 10.0, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_TRUE(IsClose(0.0, -10.0, 10.1, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 10.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 10.0, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_TRUE(IsClose(-10.0, 0.0, 10.1, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 10.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 10.0, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+
+  EXPECT_FALSE(IsClose(0, 10.0, 9.9, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 10.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 10.0, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_FALSE(IsClose(10.0, 0.0, 9.9, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 10.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 10.0, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_FALSE(IsClose(0, -10.0, 9.9, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 10.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 10.0, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_FALSE(IsClose(-10.0, 0.0, 9.9, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 10.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 10.0, kTolerance);
+}
+
+TEST(IsClose, BothParametersZero) {
+  double relative_error = -1;
+  double absolute_error = -1;
+  EXPECT_TRUE(IsClose(0.0, 0.0, 0.1, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 0.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.0, kTolerance);
+  relative_error = -1;
+  absolute_error = -1;
+  EXPECT_FALSE(IsClose(0.0, 0.0, 0.0, &relative_error, &absolute_error));
+  EXPECT_NEAR(relative_error, 0.0, kTolerance);
+  EXPECT_NEAR(absolute_error, 0.0, kTolerance);
+}
+}  // namespace internal
+}  // namespace ceres
diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc
index 7f6e829..d980ba2 100644
--- a/internal/ceres/rotation_test.cc
+++ b/internal/ceres/rotation_test.cc
@@ -32,6 +32,7 @@
 #include <limits>
 #include <string>
 #include "ceres/internal/eigen.h"
+#include "ceres/is_close.h"
 #include "ceres/internal/port.h"
 #include "ceres/jet.h"
 #include "ceres/rotation.h"
@@ -701,12 +702,7 @@
 bool IsClose(double x, double y) {
   EXPECT_FALSE(IsNaN(x));
   EXPECT_FALSE(IsNaN(y));
-  double absdiff = fabs(x - y);
-  if (x == 0 || y == 0) {
-    return absdiff <= kTolerance;
-  }
-  double reldiff = absdiff / max(fabs(x), fabs(y));
-  return reldiff <= kTolerance;
+  return internal::IsClose(x, y, kTolerance, NULL, NULL);
 }
 
 template <int N>
diff --git a/internal/ceres/solver.cc b/internal/ceres/solver.cc
index 21d3c68..5cc0a66 100644
--- a/internal/ceres/solver.cc
+++ b/internal/ceres/solver.cc
@@ -496,21 +496,28 @@
   // values provided by the user.
   program->SetParameterBlockStatePtrsToUserStatePtrs();
 
+  // If gradient_checking is enabled, wrap all cost functions in a
+  // gradient checker and install a callback that terminates if any gradient
+  // error is detected.
   scoped_ptr<internal::ProblemImpl> gradient_checking_problem;
+  internal::GradientCheckingIterationCallback gradient_checking_callback;
+  Solver::Options modified_options = options;
   if (options.check_gradients) {
+    modified_options.callbacks.push_back(&gradient_checking_callback);
     gradient_checking_problem.reset(
         CreateGradientCheckingProblemImpl(
             problem_impl,
             options.numeric_derivative_relative_step_size,
-            options.gradient_check_relative_precision));
+            options.gradient_check_relative_precision,
+            &gradient_checking_callback));
     problem_impl = gradient_checking_problem.get();
     program = problem_impl->mutable_program();
   }
 
   scoped_ptr<Preprocessor> preprocessor(
-      Preprocessor::Create(options.minimizer_type));
+      Preprocessor::Create(modified_options.minimizer_type));
   PreprocessedProblem pp;
-  const bool status = preprocessor->Preprocess(options, problem_impl, &pp);
+  const bool status = preprocessor->Preprocess(modified_options, problem_impl, &pp);
   summary->fixed_cost = pp.fixed_cost;
   summary->preprocessor_time_in_seconds = WallTimeInSeconds() - start_time;
 
@@ -535,6 +542,13 @@
   summary->postprocessor_time_in_seconds =
       WallTimeInSeconds() - postprocessor_start_time;
 
+  // If the gradient checker reported an error, we want to report FAILURE
+  // instead of USER_FAILURE and provide the error log.
+  if (gradient_checking_callback.gradient_error_detected()) {
+    summary->termination_type = FAILURE;
+    summary->message = gradient_checking_callback.error_log();
+  }
+
   summary->total_time_in_seconds = WallTimeInSeconds() - start_time;
 }