forked from bartvdbraak/blender
6fb6a08bf8
Even tho it's currently only used by Libmv we might use it for something else in the future. Plus, it's actually where it logically belongs to.
338 lines
14 KiB
C++
338 lines
14 KiB
C++
// 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: keir@google.com (Keir Mierle)
|
|
|
|
#include "ceres/gradient_checking_cost_function.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <numeric>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include "ceres/cost_function.h"
|
|
#include "ceres/internal/eigen.h"
|
|
#include "ceres/internal/scoped_ptr.h"
|
|
#include "ceres/parameter_block.h"
|
|
#include "ceres/problem.h"
|
|
#include "ceres/problem_impl.h"
|
|
#include "ceres/program.h"
|
|
#include "ceres/residual_block.h"
|
|
#include "ceres/dynamic_numeric_diff_cost_function.h"
|
|
#include "ceres/stringprintf.h"
|
|
#include "ceres/types.h"
|
|
#include "glog/logging.h"
|
|
|
|
namespace ceres {
|
|
namespace internal {
|
|
|
|
using std::abs;
|
|
using std::max;
|
|
using std::string;
|
|
using std::vector;
|
|
|
|
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)
|
|
: function_(function),
|
|
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);
|
|
|
|
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() { }
|
|
|
|
virtual bool Evaluate(double const* const* parameters,
|
|
double* residuals,
|
|
double** jacobians) const {
|
|
if (!jacobians) {
|
|
// Nothing to check in this case; just forward.
|
|
return function_->Evaluate(parameters, residuals, NULL);
|
|
}
|
|
|
|
int num_residuals = function_->num_residuals();
|
|
|
|
// 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.";
|
|
return false;
|
|
}
|
|
|
|
// Evaluate the derivative using numeric derivatives.
|
|
finite_diff_cost_function_->Evaluate(
|
|
parameters,
|
|
residuals,
|
|
&finite_difference_jacobian_pointers[0]);
|
|
|
|
// 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;
|
|
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";
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
const CostFunction* function_;
|
|
internal::scoped_ptr<CostFunction> finite_diff_cost_function_;
|
|
double relative_precision_;
|
|
string extra_info_;
|
|
};
|
|
|
|
} // namespace
|
|
|
|
CostFunction *CreateGradientCheckingCostFunction(
|
|
const CostFunction *cost_function,
|
|
double relative_step_size,
|
|
double relative_precision,
|
|
const string& extra_info) {
|
|
NumericDiffOptions numeric_diff_options;
|
|
numeric_diff_options.relative_step_size = relative_step_size;
|
|
|
|
return new GradientCheckingCostFunction(cost_function,
|
|
numeric_diff_options,
|
|
relative_precision,
|
|
extra_info);
|
|
}
|
|
|
|
ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl,
|
|
double relative_step_size,
|
|
double relative_precision) {
|
|
// 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
|
|
// LossFunctions and LocalParameterizations are reused and since
|
|
// they are owned by problem_impl, gradient_checking_problem_impl
|
|
// should not take ownership of it.
|
|
Problem::Options gradient_checking_problem_options;
|
|
gradient_checking_problem_options.cost_function_ownership = TAKE_OWNERSHIP;
|
|
gradient_checking_problem_options.loss_function_ownership =
|
|
DO_NOT_TAKE_OWNERSHIP;
|
|
gradient_checking_problem_options.local_parameterization_ownership =
|
|
DO_NOT_TAKE_OWNERSHIP;
|
|
|
|
ProblemImpl* gradient_checking_problem_impl = new ProblemImpl(
|
|
gradient_checking_problem_options);
|
|
|
|
Program* program = problem_impl->mutable_program();
|
|
|
|
// For every ParameterBlock in problem_impl, create a new parameter
|
|
// block with the same local parameterization and constancy.
|
|
const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();
|
|
for (int i = 0; i < parameter_blocks.size(); ++i) {
|
|
ParameterBlock* parameter_block = parameter_blocks[i];
|
|
gradient_checking_problem_impl->AddParameterBlock(
|
|
parameter_block->mutable_user_state(),
|
|
parameter_block->Size(),
|
|
parameter_block->mutable_local_parameterization());
|
|
|
|
if (parameter_block->IsConstant()) {
|
|
gradient_checking_problem_impl->SetParameterBlockConstant(
|
|
parameter_block->mutable_user_state());
|
|
}
|
|
}
|
|
|
|
// For every ResidualBlock in problem_impl, create a new
|
|
// ResidualBlock by wrapping its CostFunction inside a
|
|
// GradientCheckingCostFunction.
|
|
const vector<ResidualBlock*>& residual_blocks = program->residual_blocks();
|
|
for (int i = 0; i < residual_blocks.size(); ++i) {
|
|
ResidualBlock* residual_block = residual_blocks[i];
|
|
|
|
// Build a human readable string which identifies the
|
|
// ResidualBlock. This is used by the GradientCheckingCostFunction
|
|
// when logging debugging information.
|
|
string extra_info = StringPrintf(
|
|
"Residual block id %d; depends on parameters [", i);
|
|
vector<double*> parameter_blocks;
|
|
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) ? ", " : "]";
|
|
}
|
|
|
|
// Wrap the original CostFunction in a GradientCheckingCostFunction.
|
|
CostFunction* gradient_checking_cost_function =
|
|
CreateGradientCheckingCostFunction(residual_block->cost_function(),
|
|
relative_step_size,
|
|
relative_precision,
|
|
extra_info);
|
|
|
|
// The const_cast is necessary because
|
|
// ProblemImpl::AddResidualBlock can potentially take ownership of
|
|
// the LossFunction, but in this case we are guaranteed that this
|
|
// will not be the case, so this const_cast is harmless.
|
|
gradient_checking_problem_impl->AddResidualBlock(
|
|
gradient_checking_cost_function,
|
|
const_cast<LossFunction*>(residual_block->loss_function()),
|
|
parameter_blocks);
|
|
}
|
|
|
|
// Normally, when a problem is given to the solver, we guarantee
|
|
// that the state pointers for each parameter block point to the
|
|
// user provided data. Since we are creating this new problem from a
|
|
// problem given to us at an arbitrary stage of the solve, we cannot
|
|
// depend on this being the case, so we explicitly call
|
|
// SetParameterBlockStatePtrsToUserStatePtrs to ensure that this is
|
|
// the case.
|
|
gradient_checking_problem_impl
|
|
->mutable_program()
|
|
->SetParameterBlockStatePtrsToUserStatePtrs();
|
|
|
|
return gradient_checking_problem_impl;
|
|
}
|
|
|
|
|
|
} // namespace internal
|
|
} // namespace ceres
|