Gradient checking cleanup and local parameterization bugfix

Change the Ceres gradient checking API to make is useful for
unit testing, clean up code duplication and fix interaction between
gradient checking and local parameterizations.

There were two gradient checking implementations, one being used
when using the check_gradients flag in the Solver, the other
being a standalone class. The standalone version was restricted
to cost functions with fixed parameter sizes at compile time, which
is being lifted here. This enables it to be used inside the
GradientCheckingCostFunction as well.

In addition, this installs new hooks in the Solver to ensure
that Solve will fail if any incorrect gradients are detected. This
way, you can set the check_gradient flags to true and detect
errors in an automated way, instead of just printing error information
to the log. The error log is now also returned in the Solver summary
instead of being printed directly. The user can then decide what to
do with it. The existing hooks for user callbacks are used for
this purpose to keep the internal API changes minimal and non-invasive.

The last and biggest change is the way the the interaction between
local parameterizations and the gradient checker works. Before,
local parameterizations would be ignored by the checker. However,
if a cost function does not compute its Jacobian along the null
space of the local parameterization, this wil not have any effect
on the solver, but would result in a gradient checker error.
With this change, the Jacobians are multiplied by the Jacobians
of the respective local parameterization and thus being compared
in the tangent space only.

The typical use case for this are quaternion parameters, where
a cost function will typically assume that the quaternion is
always normalized, skipping the correct computation of the Jacobian
along the normal to save computation cost.

Change-Id: I5e1bb97b8a899436cea25101efe5011b0bb13282

internal/ceres/solver.cc

@@ -496,21 +496,28 @@ void Solver::Solve(const Solver::Options& options,
   // 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 @@ void Solver::Solve(const Solver::Options& options,
   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;
 }