From f44907f702d1d53724064f72d18e177a4f2fbfeb Mon Sep 17 00:00:00 2001 From: Keir Mierle Date: Fri, 6 Jul 2012 13:52:32 -0700 Subject: [PATCH] Compute the gradient if requested in the evaluator This extends the Evaluator interface to support evaluating the gradient in addition to the residuals and jacobian, if requested. bool Evaluate(const double* state, double* cost, double* residuals, double* gradient, <----------- NEW SparseMatrix* jacobian) = 0; The ProgramEvaluator is extended to support the new gradient evaluation. This required some gymnastics around the block evaluate preparer, which now contains a scratch evaluate preparer for the case that no jacobian is requested but the gradient is. Gradient evaluation is a prerequisite for the planned suite of first order methods, including nonlinear conjugate gradient, CG_DESCENT, L-BFGS, trust region with line search, and more. This also considerably refactors the evaluator_test to make it shorter and check the results for all combinations of the optional parameters [residuals, gradient, jacobian]. Change-Id: Ic7d0fec028dc5ffebc08ee079ad04eeaf6e02582 --- internal/ceres/block_evaluate_preparer.cc | 16 +- internal/ceres/block_evaluate_preparer.h | 16 +- internal/ceres/block_jacobian_writer.cc | 5 +- internal/ceres/evaluator.h | 1 + internal/ceres/evaluator_test.cc | 650 +++++++----------- internal/ceres/program.cc | 9 + internal/ceres/program.h | 1 + internal/ceres/program_evaluator.h | 78 ++- internal/ceres/trust_region_minimizer.cc | 12 +- internal/ceres/trust_region_minimizer_test.cc | 1 + 10 files changed, 362 insertions(+), 427 deletions(-) diff --git a/internal/ceres/block_evaluate_preparer.cc b/internal/ceres/block_evaluate_preparer.cc index 05e63eb56..9edc4fa23 100644 --- a/internal/ceres/block_evaluate_preparer.cc +++ b/internal/ceres/block_evaluate_preparer.cc @@ -40,16 +40,26 @@ namespace ceres { namespace internal { -void BlockEvaluatePreparer::Init(int** jacobian_layout) { +void BlockEvaluatePreparer::Init(int const* const* jacobian_layout, + int max_derivatives_per_residual_block) { jacobian_layout_ = jacobian_layout; + scratch_evaluate_preparer_.Init(max_derivatives_per_residual_block); } // Point the jacobian blocks directly into the block sparse matrix. void BlockEvaluatePreparer::Prepare(const ResidualBlock* residual_block, int residual_block_index, SparseMatrix* jacobian, - double** jacobians) const { - CHECK(jacobian != NULL); + double** jacobians) { + // If the overall jacobian is not available, use the scratch space. + if (jacobian == NULL) { + scratch_evaluate_preparer_.Prepare(residual_block, + residual_block_index, + jacobian, + jacobians); + return; + } + double* jacobian_values = down_cast(jacobian)->mutable_values(); diff --git a/internal/ceres/block_evaluate_preparer.h b/internal/ceres/block_evaluate_preparer.h index a7869311e..354acc031 100644 --- a/internal/ceres/block_evaluate_preparer.h +++ b/internal/ceres/block_evaluate_preparer.h @@ -36,6 +36,8 @@ #ifndef CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ #define CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ +#include "ceres/scratch_evaluate_preparer.h" + namespace ceres { namespace internal { @@ -47,18 +49,26 @@ class BlockEvaluatePreparer { // Using Init() instead of a constructor allows for allocating this structure // with new[]. This is because C++ doesn't allow passing arguments to objects // constructed with new[] (as opposed to plain 'new'). - void Init(int** jacobian_layout); + void Init(int const* const* jacobian_layout, + int max_derivatives_per_residual_block); // EvaluatePreparer interface - // Point the jacobian blocks directly into the block sparse matrix. + // Point the jacobian blocks directly into the block sparse matrix, if + // jacobian is non-null. Otherwise, uses an internal per-thread buffer to + // store the jacobians temporarily. void Prepare(const ResidualBlock* residual_block, int residual_block_index, SparseMatrix* jacobian, - double** jacobians) const; + double** jacobians); private: int const* const* jacobian_layout_; + + // For the case that the overall jacobian is not available, but the + // individual jacobians are requested, use a pass-through scratch evaluate + // preparer. + ScratchEvaluatePreparer scratch_evaluate_preparer_; }; } // namespace internal diff --git a/internal/ceres/block_jacobian_writer.cc b/internal/ceres/block_jacobian_writer.cc index 52a58bb43..f90c350cc 100644 --- a/internal/ceres/block_jacobian_writer.cc +++ b/internal/ceres/block_jacobian_writer.cc @@ -136,9 +136,12 @@ BlockJacobianWriter::BlockJacobianWriter(const Evaluator::Options& options, // makes the final Write() a nop. BlockEvaluatePreparer* BlockJacobianWriter::CreateEvaluatePreparers( int num_threads) { + int max_derivatives_per_residual_block = + program_->MaxDerivativesPerResidualBlock(); + BlockEvaluatePreparer* preparers = new BlockEvaluatePreparer[num_threads]; for (int i = 0; i < num_threads; i++) { - preparers[i].Init(&jacobian_layout_[0]); + preparers[i].Init(&jacobian_layout_[0], max_derivatives_per_residual_block); } return preparers; } diff --git a/internal/ceres/evaluator.h b/internal/ceres/evaluator.h index adefdd266..e80efa1c5 100644 --- a/internal/ceres/evaluator.h +++ b/internal/ceres/evaluator.h @@ -95,6 +95,7 @@ class Evaluator { virtual bool Evaluate(const double* state, double* cost, double* residuals, + double* gradient, SparseMatrix* jacobian) = 0; // Make a change delta (of size NumEffectiveParameters()) to state (of size diff --git a/internal/ceres/evaluator_test.cc b/internal/ceres/evaluator_test.cc index b7381a447..6fbb759e8 100644 --- a/internal/ceres/evaluator_test.cc +++ b/internal/ceres/evaluator_test.cc @@ -88,6 +88,15 @@ class ParameterIgnoringCostFunction } }; +struct ExpectedEvaluation { + int num_rows; + int num_cols; + double cost; + const double residuals[50]; + const double gradient[50]; + const double jacobian[200]; +}; + struct EvaluatorTest : public ::testing::TestWithParam > { Evaluator* CreateEvaluator(Program* program) { @@ -103,6 +112,93 @@ struct EvaluatorTest string error; return Evaluator::Create(options, program, &error); } + + void CheckEvaluation(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(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 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 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)); + + EXPECT_EQ(expected_cost, cost); + + if (expected_residuals != NULL) { + for (int i = 0; i < num_residuals; ++i) { + EXPECT_EQ(expected_residuals[i], residuals[i]) << i; + } + } + + if (expected_gradient != NULL) { + ConstVectorRef expected_gradient_vector(expected_gradient, + expected_num_cols); + + EXPECT_TRUE((gradient.array() == + expected_gradient_vector.array()).all()) + << "Actual:\n" << gradient.transpose() + << "\nExpected:\n" << expected_gradient_vector.transpose(); + } + + if (expected_jacobian != NULL) { + ConstMatrixRef expected_jacobian_matrix(expected_jacobian, + expected_num_rows, + expected_num_cols); + Matrix actual_jacobian; + jacobian->ToDenseMatrix(&actual_jacobian); + + EXPECT_TRUE((actual_jacobian.array() == + expected_jacobian_matrix.array()).all()) + << "Actual:\n" << actual_jacobian + << "\nExpected:\n" << expected_jacobian; + } + } + + // Try all combinations of parameters for the evaluator. + void CheckAllEvaluationCombinations(const ExpectedEvaluation &expected) { + for (int i = 0; i < 8; ++i) { + CheckEvaluation(&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) { @@ -111,77 +207,32 @@ void SetSparseMatrixConstant(SparseMatrix* sparse_matrix, double value) { } TEST_P(EvaluatorTest, SingleResidualProblem) { - ProblemImpl problem; - - // The values are ignored completely by the cost function. - double x[2]; - double y[3]; - double z[4]; - double state[9]; - problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 2, 3, 4>, NULL, x, y, z); - scoped_ptr evaluator(CreateEvaluator(problem.mutable_program())); - scoped_ptr jacobian(evaluator->CreateJacobian()); - ASSERT_EQ(3, jacobian->num_rows()); - ASSERT_EQ(9, jacobian->num_cols()); - - // Cost only; no residuals and no jacobian. - { - double cost = -1; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, NULL, NULL)); - EXPECT_EQ(7.0, cost); - } - - // Cost and residuals, no jacobian. - { - double cost = -1; - double residuals[3] = { -2, -2, -2 }; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL)); - EXPECT_EQ(7.0, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(3.0, residuals[2]); - } - - // Cost, residuals, and jacobian. - { - double cost = -1; - double residuals[3] = { -2, -2, -2 }; - SetSparseMatrixConstant(jacobian.get(), -1); - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, jacobian.get())); - EXPECT_EQ(7.0, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(3.0, residuals[2]); - - Matrix actual_jacobian; - jacobian->ToDenseMatrix(&actual_jacobian); - - Matrix expected_jacobian(3, 9); - expected_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; - - EXPECT_TRUE((actual_jacobian.array() == expected_jacobian.array()).all()) - << "Actual:\n" << actual_jacobian - << "\nExpected:\n" << expected_jacobian; - } + 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) { - ProblemImpl problem; - - // The values are ignored completely by the cost function. - double x[2]; - double y[3]; - double z[4]; - double state[9]; - // Add the parameters in explicit order to force the ordering in the program. problem.AddParameterBlock(x, 2); problem.AddParameterBlock(y, 3); @@ -197,143 +248,76 @@ TEST_P(EvaluatorTest, SingleResidualProblemWithPermutedParameters) { NULL, z, y, x); - scoped_ptr evaluator(CreateEvaluator(problem.mutable_program())); - scoped_ptr jacobian(evaluator->CreateJacobian()); - ASSERT_EQ(3, jacobian->num_rows()); - ASSERT_EQ(9, jacobian->num_cols()); - - // Cost only; no residuals and no jacobian. - { - double cost = -1; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, NULL, NULL)); - EXPECT_EQ(7.0, cost); - } - - // Cost and residuals, no jacobian. - { - double cost = -1; - double residuals[3] = { -2, -2, -2 }; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL)); - EXPECT_EQ(7.0, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(3.0, residuals[2]); - } - - // Cost, residuals, and jacobian. - { - double cost = -1; - double residuals[3] = { -2, -2, -2 }; - SetSparseMatrixConstant(jacobian.get(), -1); - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, jacobian.get())); - EXPECT_EQ(7.0, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(3.0, residuals[2]); - - Matrix actual_jacobian; - jacobian->ToDenseMatrix(&actual_jacobian); - - Matrix expected_jacobian(3, 9); - expected_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; - - EXPECT_TRUE((actual_jacobian.array() == expected_jacobian.array()).all()) - << "Actual:\n" << actual_jacobian - << "\nExpected:\n" << expected_jacobian; - } + 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) { - ProblemImpl problem; - - // The values are ignored completely by the cost function. - double x[2]; - double y[3]; - double z[4]; - double state[9]; - // These parameters are not used. - double w1[2]; - double w2[1]; - double w3[1]; - double w4[3]; + 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(w1, 2); - problem.AddParameterBlock(x, 2); - problem.AddParameterBlock(w2, 1); - problem.AddParameterBlock(y, 3); - problem.AddParameterBlock(w3, 1); - problem.AddParameterBlock(z, 4); - problem.AddParameterBlock(w4, 3); + 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); - scoped_ptr evaluator(CreateEvaluator(problem.mutable_program())); - scoped_ptr jacobian(evaluator->CreateJacobian()); - ASSERT_EQ(3, jacobian->num_rows()); - ASSERT_EQ(16, jacobian->num_cols()); - - // Cost only; no residuals and no jacobian. - { - double cost = -1; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, NULL, NULL)); - EXPECT_EQ(7.0, cost); - } - - // Cost and residuals, no jacobian. - { - double cost = -1; - double residuals[3] = { -2, -2, -2 }; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL)); - EXPECT_EQ(7.0, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(3.0, residuals[2]); - } - - // Cost, residuals, and jacobian. - { - double cost = -1; - double residuals[3] = { -2, -2, -2 }; - SetSparseMatrixConstant(jacobian.get(), -1); - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, jacobian.get())); - EXPECT_EQ(7.0, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(3.0, residuals[2]); - - Matrix actual_jacobian; - jacobian->ToDenseMatrix(&actual_jacobian); - - Matrix expected_jacobian(3, 16); - expected_jacobian - // w1 x w2 y w2 z w3 - << 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; - - EXPECT_TRUE((actual_jacobian.array() == expected_jacobian.array()).all()) - << "Actual:\n" << actual_jacobian - << "\nExpected:\n" << expected_jacobian; - } + 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) { - ProblemImpl problem; - - // The values are ignored completely by the cost function. - double x[2]; - double y[3]; - double z[4]; - double state[9]; - // Add the parameters in explicit order to force the ordering in the program. problem.AddParameterBlock(x, 2); problem.AddParameterBlock(y, 3); @@ -354,63 +338,22 @@ TEST_P(EvaluatorTest, MultipleResidualProblem) { NULL, y, z); - scoped_ptr evaluator(CreateEvaluator(problem.mutable_program())); - scoped_ptr jacobian(evaluator->CreateJacobian()); - ASSERT_EQ(9, jacobian->num_rows()); - ASSERT_EQ(9, jacobian->num_cols()); - - // f g h - double expected_cost = (1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0; - - - // Cost only; no residuals and no jacobian. - { - double cost = -1; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, NULL, NULL)); - EXPECT_EQ(expected_cost, cost); - } - - // Cost and residuals, no jacobian. - { - double cost = -1; - double residuals[9] = { -2, -2, -2, -2, -2, -2, -2, -2, -2 }; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL)); - EXPECT_EQ(expected_cost, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(1.0, residuals[2]); - EXPECT_EQ(2.0, residuals[3]); - EXPECT_EQ(3.0, residuals[4]); - EXPECT_EQ(1.0, residuals[5]); - EXPECT_EQ(2.0, residuals[6]); - EXPECT_EQ(3.0, residuals[7]); - EXPECT_EQ(4.0, residuals[8]); - } - - // Cost, residuals, and jacobian. - { - double cost = -1; - double residuals[9] = { -2, -2, -2, -2, -2, -2, -2, -2, -2 }; - SetSparseMatrixConstant(jacobian.get(), -1); - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, jacobian.get())); - EXPECT_EQ(expected_cost, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(1.0, residuals[2]); - EXPECT_EQ(2.0, residuals[3]); - EXPECT_EQ(3.0, residuals[4]); - EXPECT_EQ(1.0, residuals[5]); - EXPECT_EQ(2.0, residuals[6]); - EXPECT_EQ(3.0, residuals[7]); - EXPECT_EQ(4.0, residuals[8]); - - Matrix actual_jacobian; - jacobian->ToDenseMatrix(&actual_jacobian); - - Matrix expected_jacobian(9, 9); - expected_jacobian << + 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, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 4.0 }, + // 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, + { /* 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, @@ -420,23 +363,13 @@ TEST_P(EvaluatorTest, MultipleResidualProblem) { /* 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; - - EXPECT_TRUE((actual_jacobian.array() == expected_jacobian.array()).all()) - << "Actual:\n" << actual_jacobian - << "\nExpected:\n" << expected_jacobian; - } + 0, 0, 3, 6, 9, 3, 6, 9, 12 + } + }; + CheckAllEvaluationCombinations(expected); } TEST_P(EvaluatorTest, MultipleResidualsWithLocalParameterizations) { - ProblemImpl problem; - - // The values are ignored completely by the cost function. - double x[2]; - double y[3]; - double z[4]; - double state[9]; - // Add the parameters in explicit order to force the ordering in the program. problem.AddParameterBlock(x, 2); @@ -465,64 +398,22 @@ TEST_P(EvaluatorTest, MultipleResidualsWithLocalParameterizations) { NULL, y, z); - scoped_ptr evaluator(CreateEvaluator(problem.mutable_program())); - scoped_ptr jacobian(evaluator->CreateJacobian()); - ASSERT_EQ(9, jacobian->num_rows()); - ASSERT_EQ(7, jacobian->num_cols()); - - // f g h - double expected_cost = (1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0; - - - // Cost only; no residuals and no jacobian. - { - double cost = -1; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, NULL, NULL)); - EXPECT_EQ(expected_cost, cost); - } - - // Cost and residuals, no jacobian. - { - double cost = -1; - double residuals[9] = { -2, -2, -2, -2, -2, -2, -2, -2, -2 }; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL)); - EXPECT_EQ(expected_cost, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(1.0, residuals[2]); - EXPECT_EQ(2.0, residuals[3]); - EXPECT_EQ(3.0, residuals[4]); - EXPECT_EQ(1.0, residuals[5]); - EXPECT_EQ(2.0, residuals[6]); - EXPECT_EQ(3.0, residuals[7]); - EXPECT_EQ(4.0, residuals[8]); - } - - // Cost, residuals, and jacobian. - { - double cost = -1; - double residuals[9] = { -2, -2, -2, -2, -2, -2, -2, -2, -2 }; - SetSparseMatrixConstant(jacobian.get(), -1); - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, jacobian.get())); - EXPECT_EQ(expected_cost, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(1.0, residuals[2]); - EXPECT_EQ(2.0, residuals[3]); - EXPECT_EQ(3.0, residuals[4]); - EXPECT_EQ(1.0, residuals[5]); - EXPECT_EQ(2.0, residuals[6]); - EXPECT_EQ(3.0, residuals[7]); - EXPECT_EQ(4.0, residuals[8]); - - Matrix actual_jacobian; - jacobian->ToDenseMatrix(&actual_jacobian); - - // Note y and z are missing columns due to the subset parameterization. - Matrix expected_jacobian(9, 7); - expected_jacobian << - // x y z - /* f(x, y) */ 1, 2, 2, 3, 0, 0, 0, + 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, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 4.0 }, + // 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, @@ -532,17 +423,13 @@ TEST_P(EvaluatorTest, MultipleResidualsWithLocalParameterizations) { /* 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; - - EXPECT_TRUE((actual_jacobian.array() == expected_jacobian.array()).all()) - << "Actual:\n" << actual_jacobian - << "\nExpected:\n" << expected_jacobian; - } + 0, 0, 6, 9, 3, 9, 12 + } + }; + CheckAllEvaluationCombinations(expected); } TEST_P(EvaluatorTest, MultipleResidualProblemWithSomeConstantParameters) { - ProblemImpl problem; - // The values are ignored completely by the cost function. double x[2]; double y[3]; @@ -576,71 +463,30 @@ TEST_P(EvaluatorTest, MultipleResidualProblemWithSomeConstantParameters) { // 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; - *reduced_program.mutable_residual_blocks() = - problem.program().residual_blocks(); - *reduced_program.mutable_parameter_blocks() = - problem.program().parameter_blocks(); + vector* parameter_blocks = + problem.mutable_program()->mutable_parameter_blocks(); - // "z" is the last parameter; pop it off. - reduced_program.mutable_parameter_blocks()->pop_back(); + // "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(); - scoped_ptr evaluator(CreateEvaluator(&reduced_program)); - scoped_ptr jacobian(evaluator->CreateJacobian()); - ASSERT_EQ(9, jacobian->num_rows()); - ASSERT_EQ(5, jacobian->num_cols()); - - // f g h - double expected_cost = (1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0; - - - // Cost only; no residuals and no jacobian. - { - double cost = -1; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, NULL, NULL)); - EXPECT_EQ(expected_cost, cost); - } - - // Cost and residuals, no jacobian. - { - double cost = -1; - double residuals[9] = { -2, -2, -2, -2, -2, -2, -2, -2, -2 }; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL)); - EXPECT_EQ(expected_cost, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(1.0, residuals[2]); - EXPECT_EQ(2.0, residuals[3]); - EXPECT_EQ(3.0, residuals[4]); - EXPECT_EQ(1.0, residuals[5]); - EXPECT_EQ(2.0, residuals[6]); - EXPECT_EQ(3.0, residuals[7]); - EXPECT_EQ(4.0, residuals[8]); - } - - // Cost, residuals, and jacobian. - { - double cost = -1; - double residuals[9] = { -2, -2, -2, -2, -2, -2, -2, -2, -2 }; - SetSparseMatrixConstant(jacobian.get(), -1); - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, jacobian.get())); - EXPECT_EQ(expected_cost, cost); - EXPECT_EQ(1.0, residuals[0]); - EXPECT_EQ(2.0, residuals[1]); - EXPECT_EQ(1.0, residuals[2]); - EXPECT_EQ(2.0, residuals[3]); - EXPECT_EQ(3.0, residuals[4]); - EXPECT_EQ(1.0, residuals[5]); - EXPECT_EQ(2.0, residuals[6]); - EXPECT_EQ(3.0, residuals[7]); - EXPECT_EQ(4.0, residuals[8]); - - Matrix actual_jacobian; - jacobian->ToDenseMatrix(&actual_jacobian); - - Matrix expected_jacobian(9, 5); - expected_jacobian << - // x y - /* f(x, y) */ 1, 2, 1, 2, 3, + 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, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 4.0 }, + // 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, @@ -650,31 +496,27 @@ TEST_P(EvaluatorTest, MultipleResidualProblemWithSomeConstantParameters) { /* h(y, z) */ 0, 0, 3, 6, 9, 0, 0, 3, 6, 9, 0, 0, 3, 6, 9, - 0, 0, 3, 6, 9; + 0, 0, 3, 6, 9 + } + }; + CheckAllEvaluationCombinations(expected); - EXPECT_TRUE((actual_jacobian.array() == expected_jacobian.array()).all()) - << "Actual:\n" << actual_jacobian - << "\nExpected:\n" << expected_jacobian; - } + // Restore parameter block z, so it will get freed in a consistent way. + parameter_blocks->push_back(parameter_block_z); } TEST_P(EvaluatorTest, EvaluatorAbortsForResidualsThatFailToEvaluate) { - ProblemImpl problem; - - // The values are ignored completely by the cost function. - double x[2]; - double y[3]; - double z[4]; - double state[9]; - // 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(CreateEvaluator(problem.mutable_program())); scoped_ptr jacobian(evaluator->CreateJacobian()); double cost; - EXPECT_FALSE(evaluator->Evaluate(state, &cost, NULL, NULL)); + EXPECT_FALSE(evaluator->Evaluate(state, &cost, NULL, NULL, NULL)); } // In the pairs, the first argument is the linear solver type, and the second @@ -762,7 +604,7 @@ TEST(Evaluator, EvaluatorRespectsParameterChanges) { // Cost only; no residuals and no jacobian. { double cost = -1; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, NULL, NULL)); + ASSERT_TRUE(evaluator->Evaluate(state, &cost, NULL, NULL, NULL)); EXPECT_EQ(48.5, cost); } @@ -770,7 +612,7 @@ TEST(Evaluator, EvaluatorRespectsParameterChanges) { { double cost = -1; double residuals[2] = { -2, -2 }; - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL)); + ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL, NULL)); EXPECT_EQ(48.5, cost); EXPECT_EQ(4, residuals[0]); EXPECT_EQ(9, residuals[1]); @@ -781,7 +623,7 @@ TEST(Evaluator, EvaluatorRespectsParameterChanges) { double cost = -1; double residuals[2] = { -2, -2}; SetSparseMatrixConstant(jacobian.get(), -1); - ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, jacobian.get())); + 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]); diff --git a/internal/ceres/program.cc b/internal/ceres/program.cc index 3d62272ec..f6704e44a 100644 --- a/internal/ceres/program.cc +++ b/internal/ceres/program.cc @@ -201,6 +201,15 @@ int Program::MaxParametersPerResidualBlock() const { return max_parameters; } +int Program::MaxResidualsPerResidualBlock() const { + int max_residuals = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + max_residuals = max(max_residuals, + residual_blocks_[i]->NumResiduals()); + } + return max_residuals; +} + bool Program::Evaluate(double* cost, double* residuals) { *cost = 0.0; diff --git a/internal/ceres/program.h b/internal/ceres/program.h index 1386d3c6c..61fa06b6c 100644 --- a/internal/ceres/program.h +++ b/internal/ceres/program.h @@ -108,6 +108,7 @@ class Program { int MaxScratchDoublesNeededForEvaluate() const; int MaxDerivativesPerResidualBlock() const; int MaxParametersPerResidualBlock() const; + int MaxResidualsPerResidualBlock() const; // Evaluate the cost and maybe the residuals for the program. If residuals is // NULL, then residuals are not calculated. If the jacobian is needed, instead diff --git a/internal/ceres/program_evaluator.h b/internal/ceres/program_evaluator.h index b309aadfb..6c48e7d76 100644 --- a/internal/ceres/program_evaluator.h +++ b/internal/ceres/program_evaluator.h @@ -120,6 +120,7 @@ class ProgramEvaluator : public Evaluator { bool Evaluate(const double* state, double* cost, double* residuals, + double* gradient, SparseMatrix* jacobian) { // The parameters are stateful, so set the state before evaluating. if (!program_->StateVectorToParameterBlocks(state)) { @@ -162,13 +163,16 @@ class ProgramEvaluator : public Evaluator { // Prepare block residuals if requested. const ResidualBlock* residual_block = program_->residual_blocks()[i]; - double* block_residuals = (residuals != NULL) - ? (residuals + residual_layout_[i]) - : NULL; + double* block_residuals = NULL; + if (residuals != NULL) { + block_residuals = residuals + residual_layout_[i]; + } else if (gradient != NULL) { + block_residuals = scratch->residual_block_residuals.get(); + } // Prepare block jacobians if requested. double** block_jacobians = NULL; - if (jacobian != NULL) { + if (jacobian != NULL || gradient != NULL) { preparer->Prepare(residual_block, i, jacobian, @@ -178,10 +182,11 @@ class ProgramEvaluator : public Evaluator { // Evaluate the cost, residuals, and jacobians. double block_cost; - if (!residual_block->Evaluate(&block_cost, - block_residuals, - block_jacobians, - scratch->scratch.get())) { + if (!residual_block->Evaluate( + &block_cost, + block_residuals, + block_jacobians, + scratch->residual_block_evaluate_scratch.get())) { abort = true; // This ensures that the OpenMP threads have a consistent view of 'abort'. Do // the flush inside the failure case so that there is usually only one @@ -192,19 +197,49 @@ class ProgramEvaluator : public Evaluator { scratch->cost += block_cost; + // Store the jacobians, if they were requested. if (jacobian != NULL) { jacobian_writer_.Write(i, residual_layout_[i], block_jacobians, jacobian); } + + // Compute and store the gradient, if it was requested. + if (gradient != NULL) { + int num_residuals = residual_block->NumResiduals(); + int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (parameter_block->IsConstant()) { + continue; + } + MatrixRef block_jacobian(block_jacobians[j], + num_residuals, + parameter_block->LocalSize()); + VectorRef block_gradient(scratch->gradient.get() + + parameter_block->delta_offset(), + parameter_block->LocalSize()); + VectorRef block_residual(block_residuals, num_residuals); + block_gradient += block_residual.transpose() * block_jacobian; + } + } } if (!abort) { - // Sum the cost from each thread. + // Sum the cost and gradient (if requested) from each thread. (*cost) = 0.0; + int num_parameters = program_->NumEffectiveParameters(); + if (gradient != NULL) { + VectorRef(gradient, num_parameters).setZero(); + } for (int i = 0; i < options_.num_threads; ++i) { (*cost) += evaluate_scratch_[i].cost; + if (gradient != NULL) { + VectorRef(gradient, num_parameters) += + VectorRef(evaluate_scratch_[i].gradient.get(), num_parameters); + } } } return !abort; @@ -228,16 +263,28 @@ class ProgramEvaluator : public Evaluator { } private: + // Per-thread scratch space needed to evaluate and store each residual block. struct EvaluateScratch { void Init(int max_parameters_per_residual_block, - int max_scratch_doubles_needed_for_evaluate) { + int max_scratch_doubles_needed_for_evaluate, + int max_residuals_per_residual_block, + int num_parameters) { + residual_block_evaluate_scratch.reset( + new double[max_scratch_doubles_needed_for_evaluate]); + gradient.reset(new double[num_parameters]); + VectorRef(gradient.get(), num_parameters).setZero(); + residual_block_residuals.reset( + new double[max_residuals_per_residual_block]); jacobian_block_ptrs.reset( new double*[max_parameters_per_residual_block]); - scratch.reset(new double[max_scratch_doubles_needed_for_evaluate]); } double cost; - scoped_array scratch; + scoped_array residual_block_evaluate_scratch; + // The gradient in the local parameterization. + scoped_array gradient; + // Enough space to store the residual for the largest residual block. + scoped_array residual_block_residuals; scoped_array jacobian_block_ptrs; }; @@ -260,11 +307,16 @@ class ProgramEvaluator : public Evaluator { program.MaxParametersPerResidualBlock(); int max_scratch_doubles_needed_for_evaluate = program.MaxScratchDoublesNeededForEvaluate(); + int max_residuals_per_residual_block = + program.MaxResidualsPerResidualBlock(); + int num_parameters = program.NumEffectiveParameters(); EvaluateScratch* evaluate_scratch = new EvaluateScratch[num_threads]; for (int i = 0; i < num_threads; i++) { evaluate_scratch[i].Init(max_parameters_per_residual_block, - max_scratch_doubles_needed_for_evaluate); + max_scratch_doubles_needed_for_evaluate, + max_residuals_per_residual_block, + num_parameters); } return evaluate_scratch; } diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc index c47569043..f990b8016 100644 --- a/internal/ceres/trust_region_minimizer.cc +++ b/internal/ceres/trust_region_minimizer.cc @@ -161,7 +161,7 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, // Do initial cost and Jacobian evaluation. double cost = 0.0; - if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), jacobian)) { + if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), NULL, jacobian)) { LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed."; summary->termination_type = NUMERICAL_FAILURE; return; @@ -362,7 +362,9 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, // Try this step. double new_cost; - if (!evaluator->Evaluate(x_plus_delta.data(), &new_cost, NULL, NULL)) { + if (!evaluator->Evaluate(x_plus_delta.data(), + &new_cost, + NULL, NULL, NULL)) { summary->termination_type = NUMERICAL_FAILURE; LOG(WARNING) << "Terminating: Cost evaluation failed."; return; @@ -394,7 +396,11 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, x_norm = x.norm(); // Step looks good, evaluate the residuals and Jacobian at this // point. - if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), jacobian)) { + if (!evaluator->Evaluate(x.data(), + &cost, + residuals.data(), + NULL, + jacobian)) { summary->termination_type = NUMERICAL_FAILURE; LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed."; return; diff --git a/internal/ceres/trust_region_minimizer_test.cc b/internal/ceres/trust_region_minimizer_test.cc index 03297c0e0..414567a73 100644 --- a/internal/ceres/trust_region_minimizer_test.cc +++ b/internal/ceres/trust_region_minimizer_test.cc @@ -83,6 +83,7 @@ class PowellEvaluator2 : public Evaluator { virtual bool Evaluate(const double* state, double* cost, double* residuals, + double* /* gradient */, SparseMatrix* jacobian) { double x1 = state[0]; double x2 = state[1];