saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-29 16:40:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

Gradient checking cleanup and local parameterization bugfix

Browse Source 
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
This commit is contained in:
David Gossow
2016-04-29 16:07:11 +02:00
committed by Sameer Agarwal
parent d4264ec10d
commit ac3b8e8217
14 changed files with 1265 additions and 380 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/ceres/gradient_checker.h
+82 -155
View File
@@ -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