419 lines
15 KiB
C++
419 lines
15 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 <cmath>
|
||
|
|
#include <vector>
|
||
|
|
#include "ceres/cost_function.h"
|
||
|
|
#include "ceres/internal/scoped_ptr.h"
|
||
|
|
#include "ceres/local_parameterization.h"
|
||
|
|
#include "ceres/loss_function.h"
|
||
|
|
#include "ceres/parameter_block.h"
|
||
|
|
#include "ceres/problem_impl.h"
|
||
|
|
#include "ceres/program.h"
|
||
|
|
#include "ceres/random.h"
|
||
|
|
#include "ceres/residual_block.h"
|
||
|
|
#include "ceres/sized_cost_function.h"
|
||
|
|
#include "ceres/types.h"
|
||
|
|
#include "glog/logging.h"
|
||
|
|
#include "gmock/gmock.h"
|
||
|
|
#include "gmock/mock-log.h"
|
||
|
|
#include "gtest/gtest.h"
|
||
|
|
|
||
|
|
namespace ceres {
|
||
|
|
namespace internal {
|
||
|
|
|
||
|
|
using std::vector;
|
||
|
|
using testing::AllOf;
|
||
|
|
using testing::AnyNumber;
|
||
|
|
using testing::HasSubstr;
|
||
|
|
using testing::ScopedMockLog;
|
||
|
|
using testing::_;
|
||
|
|
|
||
|
|
// Pick a (non-quadratic) function whose derivative are easy:
|
||
|
|
//
|
||
|
|
// f = exp(- a' x).
|
||
|
|
// df = - f a.
|
||
|
|
//
|
||
|
|
// where 'a' is a vector of the same size as 'x'. In the block
|
||
|
|
// version, they are both block vectors, of course.
|
||
|
|
template<int bad_block = 1, int bad_variable = 2>
|
||
|
|
class TestTerm : public CostFunction {
|
||
|
|
public:
|
||
|
|
// The constructor of this function needs to know the number
|
||
|
|
// of blocks desired, and the size of each block.
|
||
|
|
TestTerm(int arity, int const *dim) : arity_(arity) {
|
||
|
|
// Make 'arity' random vectors.
|
||
|
|
a_.resize(arity_);
|
||
|
|
for (int j = 0; j < arity_; ++j) {
|
||
|
|
a_[j].resize(dim[j]);
|
||
|
|
for (int u = 0; u < dim[j]; ++u) {
|
||
|
|
a_[j][u] = 2.0 * RandDouble() - 1.0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
for (int i = 0; i < arity_; i++) {
|
||
|
|
mutable_parameter_block_sizes()->push_back(dim[i]);
|
||
|
|
}
|
||
|
|
set_num_residuals(1);
|
||
|
|
}
|
||
|
|
|
||
|
|
bool Evaluate(double const* const* parameters,
|
||
|
|
double* residuals,
|
||
|
|
double** jacobians) const {
|
||
|
|
// Compute a . x.
|
||
|
|
double ax = 0;
|
||
|
|
for (int j = 0; j < arity_; ++j) {
|
||
|
|
for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
|
||
|
|
ax += a_[j][u] * parameters[j][u];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// This is the cost, but also appears as a factor
|
||
|
|
// in the derivatives.
|
||
|
|
double f = *residuals = exp(-ax);
|
||
|
|
|
||
|
|
// Accumulate 1st order derivatives.
|
||
|
|
if (jacobians) {
|
||
|
|
for (int j = 0; j < arity_; ++j) {
|
||
|
|
if (jacobians[j]) {
|
||
|
|
for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
|
||
|
|
// See comments before class.
|
||
|
|
jacobians[j][u] = - f * a_[j][u];
|
||
|
|
|
||
|
|
if (bad_block == j && bad_variable == u) {
|
||
|
|
// Whoopsiedoopsie! Deliberately introduce a faulty jacobian entry
|
||
|
|
// like what happens when users make an error in their jacobian
|
||
|
|
// computations. This should get detected.
|
||
|
|
LOG(INFO) << "Poisoning jacobian for parameter block " << j
|
||
|
|
<< ", row 0, column " << u;
|
||
|
|
jacobians[j][u] += 500;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
int arity_;
|
||
|
|
vector<vector<double> > a_;
|
||
|
|
};
|
||
|
|
|
||
|
|
TEST(GradientCheckingCostFunction, ResidualsAndJacobiansArePreservedTest) {
|
||
|
|
srand(5);
|
||
|
|
|
||
|
|
// Test with 3 blocks of size 2, 3 and 4.
|
||
|
|
int const arity = 3;
|
||
|
|
int const dim[arity] = { 2, 3, 4 };
|
||
|
|
|
||
|
|
// Make a random set of blocks.
|
||
|
|
vector<double*> parameters(arity);
|
||
|
|
for (int j = 0; j < arity; ++j) {
|
||
|
|
parameters[j] = new double[dim[j]];
|
||
|
|
for (int u = 0; u < dim[j]; ++u) {
|
||
|
|
parameters[j][u] = 2.0 * RandDouble() - 1.0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
double original_residual;
|
||
|
|
double residual;
|
||
|
|
vector<double*> original_jacobians(arity);
|
||
|
|
vector<double*> jacobians(arity);
|
||
|
|
|
||
|
|
for (int j = 0; j < arity; ++j) {
|
||
|
|
// Since residual is one dimensional the jacobians have the same
|
||
|
|
// size as the parameter blocks.
|
||
|
|
jacobians[j] = new double[dim[j]];
|
||
|
|
original_jacobians[j] = new double[dim[j]];
|
||
|
|
}
|
||
|
|
|
||
|
|
const double kRelativeStepSize = 1e-6;
|
||
|
|
const double kRelativePrecision = 1e-4;
|
||
|
|
|
||
|
|
TestTerm<-1, -1> term(arity, dim);
|
||
|
|
scoped_ptr<CostFunction> gradient_checking_cost_function(
|
||
|
|
CreateGradientCheckingCostFunction(&term,
|
||
|
|
kRelativeStepSize,
|
||
|
|
kRelativePrecision,
|
||
|
|
"Ignored."));
|
||
|
|
term.Evaluate(¶meters[0],
|
||
|
|
&original_residual,
|
||
|
|
&original_jacobians[0]);
|
||
|
|
|
||
|
|
gradient_checking_cost_function->Evaluate(¶meters[0],
|
||
|
|
&residual,
|
||
|
|
&jacobians[0]);
|
||
|
|
EXPECT_EQ(original_residual, residual);
|
||
|
|
|
||
|
|
for (int j = 0; j < arity; j++) {
|
||
|
|
for (int k = 0; k < dim[j]; ++k) {
|
||
|
|
EXPECT_EQ(original_jacobians[j][k], jacobians[j][k]);
|
||
|
|
}
|
||
|
|
|
||
|
|
delete[] parameters[j];
|
||
|
|
delete[] jacobians[j];
|
||
|
|
delete[] original_jacobians[j];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST(GradientCheckingCostFunction, SmokeTest) {
|
||
|
|
srand(5);
|
||
|
|
|
||
|
|
// Test with 3 blocks of size 2, 3 and 4.
|
||
|
|
int const arity = 3;
|
||
|
|
int const dim[arity] = { 2, 3, 4 };
|
||
|
|
|
||
|
|
// Make a random set of blocks.
|
||
|
|
vector<double*> parameters(arity);
|
||
|
|
for (int j = 0; j < arity; ++j) {
|
||
|
|
parameters[j] = new double[dim[j]];
|
||
|
|
for (int u = 0; u < dim[j]; ++u) {
|
||
|
|
parameters[j][u] = 2.0 * RandDouble() - 1.0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
double residual;
|
||
|
|
vector<double*> jacobians(arity);
|
||
|
|
for (int j = 0; j < arity; ++j) {
|
||
|
|
// Since residual is one dimensional the jacobians have the same size as the
|
||
|
|
// parameter blocks.
|
||
|
|
jacobians[j] = new double[dim[j]];
|
||
|
|
}
|
||
|
|
|
||
|
|
const double kRelativeStepSize = 1e-6;
|
||
|
|
const double kRelativePrecision = 1e-4;
|
||
|
|
|
||
|
|
// Should have one term that's bad, causing everything to get dumped.
|
||
|
|
LOG(INFO) << "Bad gradient";
|
||
|
|
{
|
||
|
|
TestTerm<1, 2> term(arity, dim);
|
||
|
|
scoped_ptr<CostFunction> gradient_checking_cost_function(
|
||
|
|
CreateGradientCheckingCostFunction(&term,
|
||
|
|
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(¶meters[0],
|
||
|
|
&residual,
|
||
|
|
&jacobians[0]);
|
||
|
|
}
|
||
|
|
|
||
|
|
// The gradient is correct, so no errors are reported.
|
||
|
|
LOG(INFO) << "Good gradient";
|
||
|
|
{
|
||
|
|
TestTerm<-1, -1> term(arity, dim);
|
||
|
|
scoped_ptr<CostFunction> gradient_checking_cost_function(
|
||
|
|
CreateGradientCheckingCostFunction(&term,
|
||
|
|
kRelativeStepSize,
|
||
|
|
kRelativePrecision,
|
||
|
|
"Ignored."));
|
||
|
|
|
||
|
|
ScopedMockLog log;
|
||
|
|
EXPECT_CALL(log, Log(_, _, _)).Times(0);
|
||
|
|
|
||
|
|
gradient_checking_cost_function->Evaluate(¶meters[0],
|
||
|
|
&residual,
|
||
|
|
&jacobians[0]);
|
||
|
|
}
|
||
|
|
|
||
|
|
for (int j = 0; j < arity; j++) {
|
||
|
|
delete[] parameters[j];
|
||
|
|
delete[] jacobians[j];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// The following three classes are for the purposes of defining
|
||
|
|
// function signatures. They have dummy Evaluate functions.
|
||
|
|
|
||
|
|
// Trivial cost function that accepts a single argument.
|
||
|
|
class UnaryCostFunction : public CostFunction {
|
||
|
|
public:
|
||
|
|
UnaryCostFunction(int num_residuals, int32 parameter_block_size) {
|
||
|
|
set_num_residuals(num_residuals);
|
||
|
|
mutable_parameter_block_sizes()->push_back(parameter_block_size);
|
||
|
|
}
|
||
|
|
virtual ~UnaryCostFunction() {}
|
||
|
|
|
||
|
|
virtual bool Evaluate(double const* const* parameters,
|
||
|
|
double* residuals,
|
||
|
|
double** jacobians) const {
|
||
|
|
for (int i = 0; i < num_residuals(); ++i) {
|
||
|
|
residuals[i] = 1;
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
// Trivial cost function that accepts two arguments.
|
||
|
|
class BinaryCostFunction: public CostFunction {
|
||
|
|
public:
|
||
|
|
BinaryCostFunction(int num_residuals,
|
||
|
|
int32 parameter_block1_size,
|
||
|
|
int32 parameter_block2_size) {
|
||
|
|
set_num_residuals(num_residuals);
|
||
|
|
mutable_parameter_block_sizes()->push_back(parameter_block1_size);
|
||
|
|
mutable_parameter_block_sizes()->push_back(parameter_block2_size);
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual bool Evaluate(double const* const* parameters,
|
||
|
|
double* residuals,
|
||
|
|
double** jacobians) const {
|
||
|
|
for (int i = 0; i < num_residuals(); ++i) {
|
||
|
|
residuals[i] = 2;
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
// Trivial cost function that accepts three arguments.
|
||
|
|
class TernaryCostFunction: public CostFunction {
|
||
|
|
public:
|
||
|
|
TernaryCostFunction(int num_residuals,
|
||
|
|
int32 parameter_block1_size,
|
||
|
|
int32 parameter_block2_size,
|
||
|
|
int32 parameter_block3_size) {
|
||
|
|
set_num_residuals(num_residuals);
|
||
|
|
mutable_parameter_block_sizes()->push_back(parameter_block1_size);
|
||
|
|
mutable_parameter_block_sizes()->push_back(parameter_block2_size);
|
||
|
|
mutable_parameter_block_sizes()->push_back(parameter_block3_size);
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual bool Evaluate(double const* const* parameters,
|
||
|
|
double* residuals,
|
||
|
|
double** jacobians) const {
|
||
|
|
for (int i = 0; i < num_residuals(); ++i) {
|
||
|
|
residuals[i] = 3;
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
// Verify that the two ParameterBlocks are formed from the same user
|
||
|
|
// array and have the same LocalParameterization object.
|
||
|
|
void ParameterBlocksAreEquivalent(const ParameterBlock* left,
|
||
|
|
const ParameterBlock* right) {
|
||
|
|
CHECK_NOTNULL(left);
|
||
|
|
CHECK_NOTNULL(right);
|
||
|
|
EXPECT_EQ(left->user_state(), right->user_state());
|
||
|
|
EXPECT_EQ(left->Size(), right->Size());
|
||
|
|
EXPECT_EQ(left->Size(), right->Size());
|
||
|
|
EXPECT_EQ(left->LocalSize(), right->LocalSize());
|
||
|
|
EXPECT_EQ(left->local_parameterization(), right->local_parameterization());
|
||
|
|
EXPECT_EQ(left->IsConstant(), right->IsConstant());
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST(GradientCheckingProblemImpl, ProblemDimensionsMatch) {
|
||
|
|
// Parameter blocks with arbitrarily chosen initial values.
|
||
|
|
double x[] = {1.0, 2.0, 3.0};
|
||
|
|
double y[] = {4.0, 5.0, 6.0, 7.0};
|
||
|
|
double z[] = {8.0, 9.0, 10.0, 11.0, 12.0};
|
||
|
|
double w[] = {13.0, 14.0, 15.0, 16.0};
|
||
|
|
|
||
|
|
ProblemImpl problem_impl;
|
||
|
|
problem_impl.AddParameterBlock(x, 3);
|
||
|
|
problem_impl.AddParameterBlock(y, 4);
|
||
|
|
problem_impl.SetParameterBlockConstant(y);
|
||
|
|
problem_impl.AddParameterBlock(z, 5);
|
||
|
|
problem_impl.AddParameterBlock(w, 4, new QuaternionParameterization);
|
||
|
|
problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
|
||
|
|
problem_impl.AddResidualBlock(new BinaryCostFunction(6, 5, 4) ,
|
||
|
|
NULL, z, y);
|
||
|
|
problem_impl.AddResidualBlock(new BinaryCostFunction(3, 3, 5),
|
||
|
|
new TrivialLoss, x, z);
|
||
|
|
problem_impl.AddResidualBlock(new BinaryCostFunction(7, 5, 3),
|
||
|
|
NULL, z, x);
|
||
|
|
problem_impl.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4),
|
||
|
|
NULL, z, x, y);
|
||
|
|
|
||
|
|
scoped_ptr<ProblemImpl> gradient_checking_problem_impl(
|
||
|
|
CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0));
|
||
|
|
|
||
|
|
// The dimensions of the two problems match.
|
||
|
|
EXPECT_EQ(problem_impl.NumParameterBlocks(),
|
||
|
|
gradient_checking_problem_impl->NumParameterBlocks());
|
||
|
|
EXPECT_EQ(problem_impl.NumResidualBlocks(),
|
||
|
|
gradient_checking_problem_impl->NumResidualBlocks());
|
||
|
|
|
||
|
|
EXPECT_EQ(problem_impl.NumParameters(),
|
||
|
|
gradient_checking_problem_impl->NumParameters());
|
||
|
|
EXPECT_EQ(problem_impl.NumResiduals(),
|
||
|
|
gradient_checking_problem_impl->NumResiduals());
|
||
|
|
|
||
|
|
const Program& program = problem_impl.program();
|
||
|
|
const Program& gradient_checking_program =
|
||
|
|
gradient_checking_problem_impl->program();
|
||
|
|
|
||
|
|
// 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
|
||
|
|
// anticipate that possibility not worth the extra complexity in
|
||
|
|
// this test.
|
||
|
|
for (int i = 0; i < program.parameter_blocks().size(); ++i) {
|
||
|
|
ParameterBlocksAreEquivalent(
|
||
|
|
program.parameter_blocks()[i],
|
||
|
|
gradient_checking_program.parameter_blocks()[i]);
|
||
|
|
}
|
||
|
|
|
||
|
|
for (int i = 0; i < program.residual_blocks().size(); ++i) {
|
||
|
|
// Compare the sizes of the two ResidualBlocks.
|
||
|
|
const ResidualBlock* original_residual_block =
|
||
|
|
program.residual_blocks()[i];
|
||
|
|
const ResidualBlock* new_residual_block =
|
||
|
|
gradient_checking_program.residual_blocks()[i];
|
||
|
|
EXPECT_EQ(original_residual_block->NumParameterBlocks(),
|
||
|
|
new_residual_block->NumParameterBlocks());
|
||
|
|
EXPECT_EQ(original_residual_block->NumResiduals(),
|
||
|
|
new_residual_block->NumResiduals());
|
||
|
|
EXPECT_EQ(original_residual_block->NumScratchDoublesForEvaluate(),
|
||
|
|
new_residual_block->NumScratchDoublesForEvaluate());
|
||
|
|
|
||
|
|
// Verify that the ParameterBlocks for the two residuals are equivalent.
|
||
|
|
for (int j = 0; j < original_residual_block->NumParameterBlocks(); ++j) {
|
||
|
|
ParameterBlocksAreEquivalent(
|
||
|
|
original_residual_block->parameter_blocks()[j],
|
||
|
|
new_residual_block->parameter_blocks()[j]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace internal
|
||
|
|
} // namespace ceres
|