saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-30 00:50:37 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
7bdceb46cf8e9ef2c650b50b2e5a116bea04f20f
ceres-solver/internal/ceres/evaluator_test.cc
T
Mike Vitus f408f89e8b Adds a Ceres Context structure. 
A Ceres Context holds common global state that can be re-used within
Ceres.  The Context current contains a thread pool if compiling with
C++11 threading support.  Threads are expensive to create and destroy so
it is good to maintain across multiple Ceres solves.

Tested by compiling with and without TBB support and ran unit tests. Ran
bazel as well.

Change-Id: I82f598dfae642aa0e81a6039dc174608a5e8dbfb
2018-02-26 10:37:53 -08:00

671 lines
22 KiB
C++
Raw Blame History

// 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)
//
// Tests shared across evaluators. The tests try all combinations of linear
// solver and num_eliminate_blocks (for schur-based solvers).
#include "ceres/evaluator.h"
#include "ceres/casts.h"
#include "ceres/cost_function.h"
#include "ceres/crs_matrix.h"
#include "ceres/evaluator_test_utils.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/local_parameterization.h"
#include "ceres/problem_impl.h"
#include "ceres/program.h"
#include "ceres/sized_cost_function.h"
#include "ceres/sparse_matrix.h"
#include "ceres/stringprintf.h"
#include "ceres/types.h"
#include "gtest/gtest.h"
namespace ceres {
namespace internal {
using std::string;
using std::vector;
// TODO(keir): Consider pushing this into a common test utils file.
template<int kFactor, int kNumResiduals,
int N0 = 0, int N1 = 0, int N2 = 0, bool kSucceeds = true>
class ParameterIgnoringCostFunction
: public SizedCostFunction<kNumResiduals, N0, N1, N2> {
typedef SizedCostFunction<kNumResiduals, N0, N1, N2> Base;
public:
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
for (int i = 0; i < Base::num_residuals(); ++i) {
residuals[i] = i + 1;
}
if (jacobians) {
for (int k = 0; k < Base::parameter_block_sizes().size(); ++k) {
// The jacobians here are full sized, but they are transformed in the
// evaluator into the "local" jacobian. In the tests, the "subset
// constant" parameterization is used, which should pick out columns
// from these jacobians. Put values in the jacobian that make this
// obvious; in particular, make the jacobians like this:
//
// 1 2 3 4 ...
// 1 2 3 4 ... .* kFactor
// 1 2 3 4 ...
//
// where the multiplication by kFactor makes it easier to distinguish
// between Jacobians of different residuals for the same parameter.
if (jacobians[k] != NULL) {
MatrixRef jacobian(jacobians[k],
Base::num_residuals(),
Base::parameter_block_sizes()[k]);
for (int j = 0; j < Base::parameter_block_sizes()[k]; ++j) {
jacobian.col(j).setConstant(kFactor * (j + 1));
}
}
}
}
return kSucceeds;
}
};
struct EvaluatorTestOptions {
EvaluatorTestOptions(LinearSolverType linear_solver_type,
int num_eliminate_blocks,
bool dynamic_sparsity = false)
: linear_solver_type(linear_solver_type),
num_eliminate_blocks(num_eliminate_blocks),
dynamic_sparsity(dynamic_sparsity) {}
LinearSolverType linear_solver_type;
int num_eliminate_blocks;
bool dynamic_sparsity;
};
struct EvaluatorTest
: public ::testing::TestWithParam<EvaluatorTestOptions> {
Evaluator* CreateEvaluator(Program* program) {
// This program is straight from the ProblemImpl, and so has no index/offset
// yet; compute it here as required by the evalutor implementations.
program->SetParameterOffsetsAndIndex();
if (VLOG_IS_ON(1)) {
string report;
StringAppendF(&report, "Creating evaluator with type: %d",
GetParam().linear_solver_type);
if (GetParam().linear_solver_type == SPARSE_NORMAL_CHOLESKY) {
StringAppendF(&report, ", dynamic_sparsity: %d",
GetParam().dynamic_sparsity);
}
StringAppendF(&report, " and num_eliminate_blocks: %d",
GetParam().num_eliminate_blocks);
VLOG(1) << report;
}
Evaluator::Options options;
options.linear_solver_type = GetParam().linear_solver_type;
options.num_eliminate_blocks = GetParam().num_eliminate_blocks;
options.dynamic_sparsity = GetParam().dynamic_sparsity;
options.context = problem.context();
string error;
return Evaluator::Create(options, program, &error);
}
void EvaluateAndCompare(ProblemImpl *problem,
int expected_num_rows,
int expected_num_cols,
double expected_cost,
const double* expected_residuals,
const double* expected_gradient,
const double* expected_jacobian) {
scoped_ptr<Evaluator> evaluator(
CreateEvaluator(problem->mutable_program()));
int num_residuals = expected_num_rows;
int num_parameters = expected_num_cols;
double cost = -1;
Vector residuals(num_residuals);
residuals.setConstant(-2000);
Vector gradient(num_parameters);
gradient.setConstant(-3000);
scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
ASSERT_EQ(expected_num_rows, evaluator->NumResiduals());
ASSERT_EQ(expected_num_cols, evaluator->NumEffectiveParameters());
ASSERT_EQ(expected_num_rows, jacobian->num_rows());
ASSERT_EQ(expected_num_cols, jacobian->num_cols());
vector<double> state(evaluator->NumParameters());
ASSERT_TRUE(evaluator->Evaluate(
&state[0],
&cost,
expected_residuals != NULL ? &residuals[0] : NULL,
expected_gradient != NULL ? &gradient[0] : NULL,
expected_jacobian != NULL ? jacobian.get() : NULL));
Matrix actual_jacobian;
if (expected_jacobian != NULL) {
jacobian->ToDenseMatrix(&actual_jacobian);
}
CompareEvaluations(expected_num_rows,
expected_num_cols,
expected_cost,
expected_residuals,
expected_gradient,
expected_jacobian,
cost,
&residuals[0],
&gradient[0],
actual_jacobian.data());
}
// Try all combinations of parameters for the evaluator.
void CheckAllEvaluationCombinations(const ExpectedEvaluation &expected) {
for (int i = 0; i < 8; ++i) {
EvaluateAndCompare(&problem,
expected.num_rows,
expected.num_cols,
expected.cost,
(i & 1) ? expected.residuals : NULL,
(i & 2) ? expected.gradient : NULL,
(i & 4) ? expected.jacobian : NULL);
}
}
// The values are ignored completely by the cost function.
double x[2];
double y[3];
double z[4];
ProblemImpl problem;
};
void SetSparseMatrixConstant(SparseMatrix* sparse_matrix, double value) {
VectorRef(sparse_matrix->mutable_values(),
sparse_matrix->num_nonzeros()).setConstant(value);
}
TEST_P(EvaluatorTest, SingleResidualProblem) {
problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 2, 3, 4>,
NULL,
x, y, z);
ExpectedEvaluation expected = {
// Rows/columns
3, 9,
// Cost
7.0,
// Residuals
{ 1.0, 2.0, 3.0 },
// Gradient
{ 6.0, 12.0, // x
6.0, 12.0, 18.0, // y
6.0, 12.0, 18.0, 24.0, // z
},
// Jacobian
// x y z
{ 1, 2, 1, 2, 3, 1, 2, 3, 4,
1, 2, 1, 2, 3, 1, 2, 3, 4,
1, 2, 1, 2, 3, 1, 2, 3, 4
}
};
CheckAllEvaluationCombinations(expected);
}
TEST_P(EvaluatorTest, SingleResidualProblemWithPermutedParameters) {
// Add the parameters in explicit order to force the ordering in the program.
problem.AddParameterBlock(x, 2);
problem.AddParameterBlock(y, 3);
problem.AddParameterBlock(z, 4);
// Then use a cost function which is similar to the others, but swap around
// the ordering of the parameters to the cost function. This shouldn't affect
// the jacobian evaluation, but requires explicit handling in the evaluators.
// At one point the compressed row evaluator had a bug that went undetected
// for a long time, since by chance most users added parameters to the problem
// in the same order that they occurred as parameters to a cost function.
problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 4, 3, 2>,
NULL,
z, y, x);
ExpectedEvaluation expected = {
// Rows/columns
3, 9,
// Cost
7.0,
// Residuals
{ 1.0, 2.0, 3.0 },
// Gradient
{ 6.0, 12.0, // x
6.0, 12.0, 18.0, // y
6.0, 12.0, 18.0, 24.0, // z
},
// Jacobian
// x y z
{ 1, 2, 1, 2, 3, 1, 2, 3, 4,
1, 2, 1, 2, 3, 1, 2, 3, 4,
1, 2, 1, 2, 3, 1, 2, 3, 4
}
};
CheckAllEvaluationCombinations(expected);
}
TEST_P(EvaluatorTest, SingleResidualProblemWithNuisanceParameters) {
// These parameters are not used.
double a[2];
double b[1];
double c[1];
double d[3];
// Add the parameters in a mixed order so the Jacobian is "checkered" with the
// values from the other parameters.
problem.AddParameterBlock(a, 2);
problem.AddParameterBlock(x, 2);
problem.AddParameterBlock(b, 1);
problem.AddParameterBlock(y, 3);
problem.AddParameterBlock(c, 1);
problem.AddParameterBlock(z, 4);
problem.AddParameterBlock(d, 3);
problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 2, 3, 4>,
NULL,
x, y, z);
ExpectedEvaluation expected = {
// Rows/columns
3, 16,
// Cost
7.0,
// Residuals
{ 1.0, 2.0, 3.0 },
// Gradient
{ 0.0, 0.0, // a
6.0, 12.0, // x
0.0, // b
6.0, 12.0, 18.0, // y
0.0, // c
6.0, 12.0, 18.0, 24.0, // z
0.0, 0.0, 0.0, // d
},
// Jacobian
// a x b y c z d
{ 0, 0, 1, 2, 0, 1, 2, 3, 0, 1, 2, 3, 4, 0, 0, 0,
0, 0, 1, 2, 0, 1, 2, 3, 0, 1, 2, 3, 4, 0, 0, 0,
0, 0, 1, 2, 0, 1, 2, 3, 0, 1, 2, 3, 4, 0, 0, 0
}
};
CheckAllEvaluationCombinations(expected);
}
TEST_P(EvaluatorTest, MultipleResidualProblem) {
// Add the parameters in explicit order to force the ordering in the program.
problem.AddParameterBlock(x, 2);
problem.AddParameterBlock(y, 3);
problem.AddParameterBlock(z, 4);
// f(x, y) in R^2
problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
NULL,
x, y);
// g(x, z) in R^3
problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
NULL,
x, z);
// h(y, z) in R^4
problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
NULL,
y, z);
ExpectedEvaluation expected = {
// Rows/columns
9, 9,
// Cost
// f g h
( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
// Residuals
{ 1.0, 2.0, // f
1.0, 2.0, 3.0, // g
1.0, 2.0, 3.0, 4.0 // h
},
// Gradient
{ 15.0, 30.0, // x
33.0, 66.0, 99.0, // y
42.0, 84.0, 126.0, 168.0 // z
},
// Jacobian
// x y z
{ /* f(x, y) */ 1, 2, 1, 2, 3, 0, 0, 0, 0,
1, 2, 1, 2, 3, 0, 0, 0, 0,
/* g(x, z) */ 2, 4, 0, 0, 0, 2, 4, 6, 8,
2, 4, 0, 0, 0, 2, 4, 6, 8,
2, 4, 0, 0, 0, 2, 4, 6, 8,
/* h(y, z) */ 0, 0, 3, 6, 9, 3, 6, 9, 12,
0, 0, 3, 6, 9, 3, 6, 9, 12,
0, 0, 3, 6, 9, 3, 6, 9, 12,
0, 0, 3, 6, 9, 3, 6, 9, 12
}
};
CheckAllEvaluationCombinations(expected);
}
TEST_P(EvaluatorTest, MultipleResidualsWithLocalParameterizations) {
// Add the parameters in explicit order to force the ordering in the program.
problem.AddParameterBlock(x, 2);
// Fix y's first dimension.
vector<int> y_fixed;
y_fixed.push_back(0);
problem.AddParameterBlock(y, 3, new SubsetParameterization(3, y_fixed));
// Fix z's second dimension.
vector<int> z_fixed;
z_fixed.push_back(1);
problem.AddParameterBlock(z, 4, new SubsetParameterization(4, z_fixed));
// f(x, y) in R^2
problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
NULL,
x, y);
// g(x, z) in R^3
problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
NULL,
x, z);
// h(y, z) in R^4
problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
NULL,
y, z);
ExpectedEvaluation expected = {
// Rows/columns
9, 7,
// Cost
// f g h
( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
// Residuals
{ 1.0, 2.0, // f
1.0, 2.0, 3.0, // g
1.0, 2.0, 3.0, 4.0 // h
},
// Gradient
{ 15.0, 30.0, // x
66.0, 99.0, // y
42.0, 126.0, 168.0 // z
},
// Jacobian
// x y z
{ /* f(x, y) */ 1, 2, 2, 3, 0, 0, 0,
1, 2, 2, 3, 0, 0, 0,
/* g(x, z) */ 2, 4, 0, 0, 2, 6, 8,
2, 4, 0, 0, 2, 6, 8,
2, 4, 0, 0, 2, 6, 8,
/* h(y, z) */ 0, 0, 6, 9, 3, 9, 12,
0, 0, 6, 9, 3, 9, 12,
0, 0, 6, 9, 3, 9, 12,
0, 0, 6, 9, 3, 9, 12
}
};
CheckAllEvaluationCombinations(expected);
}
TEST_P(EvaluatorTest, MultipleResidualProblemWithSomeConstantParameters) {
// The values are ignored completely by the cost function.
double x[2];
double y[3];
double z[4];
// Add the parameters in explicit order to force the ordering in the program.
problem.AddParameterBlock(x, 2);
problem.AddParameterBlock(y, 3);
problem.AddParameterBlock(z, 4);
// f(x, y) in R^2
problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
NULL,
x, y);
// g(x, z) in R^3
problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
NULL,
x, z);
// h(y, z) in R^4
problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
NULL,
y, z);
// For this test, "z" is constant.
problem.SetParameterBlockConstant(z);
// Create the reduced program which is missing the fixed "z" variable.
// Normally, the preprocessing of the program that happens in solver_impl
// takes care of this, but we don't want to invoke the solver here.
Program reduced_program;
vector<ParameterBlock*>* parameter_blocks =
problem.mutable_program()->mutable_parameter_blocks();
// "z" is the last parameter; save it for later and pop it off temporarily.
// Note that "z" will still get read during evaluation, so it cannot be
// deleted at this point.
ParameterBlock* parameter_block_z = parameter_blocks->back();
parameter_blocks->pop_back();
ExpectedEvaluation expected = {
// Rows/columns
9, 5,
// Cost
// f g h
( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
// Residuals
{ 1.0, 2.0, // f
1.0, 2.0, 3.0, // g
1.0, 2.0, 3.0, 4.0 // h
},
// Gradient
{ 15.0, 30.0, // x
33.0, 66.0, 99.0, // y
},
// Jacobian
// x y
{ /* f(x, y) */ 1, 2, 1, 2, 3,
1, 2, 1, 2, 3,
/* g(x, z) */ 2, 4, 0, 0, 0,
2, 4, 0, 0, 0,
2, 4, 0, 0, 0,
/* h(y, z) */ 0, 0, 3, 6, 9,
0, 0, 3, 6, 9,
0, 0, 3, 6, 9,
0, 0, 3, 6, 9
}
};
CheckAllEvaluationCombinations(expected);
// Restore parameter block z, so it will get freed in a consistent way.
parameter_blocks->push_back(parameter_block_z);
}
TEST_P(EvaluatorTest, EvaluatorAbortsForResidualsThatFailToEvaluate) {
// Switch the return value to failure.
problem.AddResidualBlock(
new ParameterIgnoringCostFunction<20, 3, 2, 3, 4, false>, NULL, x, y, z);
// The values are ignored.
double state[9];
scoped_ptr<Evaluator> evaluator(CreateEvaluator(problem.mutable_program()));
scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
double cost;
EXPECT_FALSE(evaluator->Evaluate(state, &cost, NULL, NULL, NULL));
}
// In the pairs, the first argument is the linear solver type, and the second
// argument is num_eliminate_blocks. Changing the num_eliminate_blocks only
// makes sense for the schur-based solvers.
//
// Try all values of num_eliminate_blocks that make sense given that in the
// tests a maximum of 4 parameter blocks are present.
INSTANTIATE_TEST_CASE_P(
LinearSolvers,
EvaluatorTest,
::testing::Values(
EvaluatorTestOptions(DENSE_QR, 0),
EvaluatorTestOptions(DENSE_SCHUR, 0),
EvaluatorTestOptions(DENSE_SCHUR, 1),
EvaluatorTestOptions(DENSE_SCHUR, 2),
EvaluatorTestOptions(DENSE_SCHUR, 3),
EvaluatorTestOptions(DENSE_SCHUR, 4),
EvaluatorTestOptions(SPARSE_SCHUR, 0),
EvaluatorTestOptions(SPARSE_SCHUR, 1),
EvaluatorTestOptions(SPARSE_SCHUR, 2),
EvaluatorTestOptions(SPARSE_SCHUR, 3),
EvaluatorTestOptions(SPARSE_SCHUR, 4),
EvaluatorTestOptions(ITERATIVE_SCHUR, 0),
EvaluatorTestOptions(ITERATIVE_SCHUR, 1),
EvaluatorTestOptions(ITERATIVE_SCHUR, 2),
EvaluatorTestOptions(ITERATIVE_SCHUR, 3),
EvaluatorTestOptions(ITERATIVE_SCHUR, 4),
EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, false),
EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, true)));
// Simple cost function used to check if the evaluator is sensitive to
// state changes.
class ParameterSensitiveCostFunction : public SizedCostFunction<2, 2> {
public:
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
double x1 = parameters[0][0];
double x2 = parameters[0][1];
residuals[0] = x1 * x1;
residuals[1] = x2 * x2;
if (jacobians != NULL) {
double* jacobian = jacobians[0];
if (jacobian != NULL) {
jacobian[0] = 2.0 * x1;
jacobian[1] = 0.0;
jacobian[2] = 0.0;
jacobian[3] = 2.0 * x2;
}
}
return true;
}
};
TEST(Evaluator, EvaluatorRespectsParameterChanges) {
ProblemImpl problem;
double x[2];
x[0] = 1.0;
x[1] = 1.0;
problem.AddResidualBlock(new ParameterSensitiveCostFunction(), NULL, x);
Program* program = problem.mutable_program();
program->SetParameterOffsetsAndIndex();
Evaluator::Options options;
options.linear_solver_type = DENSE_QR;
options.num_eliminate_blocks = 0;
options.context = problem.context();
string error;
scoped_ptr<Evaluator> evaluator(Evaluator::Create(options, program, &error));
scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
ASSERT_EQ(2, jacobian->num_rows());
ASSERT_EQ(2, jacobian->num_cols());
double state[2];
state[0] = 2.0;
state[1] = 3.0;
// The original state of a residual block comes from the user's
// state. So the original state is 1.0, 1.0, and the only way we get
// the 2.0, 3.0 results in the following tests is if it respects the
// values in the state vector.
// Cost only; no residuals and no jacobian.
{
double cost = -1;
ASSERT_TRUE(evaluator->Evaluate(state, &cost, NULL, NULL, NULL));
EXPECT_EQ(48.5, cost);
}
// Cost and residuals, no jacobian.
{
double cost = -1;
double residuals[2] = { -2, -2 };
ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL, NULL));
EXPECT_EQ(48.5, cost);
EXPECT_EQ(4, residuals[0]);
EXPECT_EQ(9, residuals[1]);
}
// Cost, residuals, and jacobian.
{
double cost = -1;
double residuals[2] = { -2, -2};
SetSparseMatrixConstant(jacobian.get(), -1);
ASSERT_TRUE(evaluator->Evaluate(state,
&cost,
residuals,
NULL,
jacobian.get()));
EXPECT_EQ(48.5, cost);
EXPECT_EQ(4, residuals[0]);
EXPECT_EQ(9, residuals[1]);
Matrix actual_jacobian;
jacobian->ToDenseMatrix(&actual_jacobian);
Matrix expected_jacobian(2, 2);
expected_jacobian
<< 2 * state[0], 0,
0, 2 * state[1];
EXPECT_TRUE((actual_jacobian.array() == expected_jacobian.array()).all())
<< "Actual:\n" << actual_jacobian
<< "\nExpected:\n" << expected_jacobian;
}
}
} // namespace internal
} // namespace ceres
Reference in New Issue View Git Blame Copy Permalink