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
diff --git a/internal/ceres/block_evaluate_preparer.cc b/internal/ceres/block_evaluate_preparer.cc
index 05e63eb..9edc4fa 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<BlockSparseMatrix*>(jacobian)->mutable_values();
diff --git a/internal/ceres/block_evaluate_preparer.h b/internal/ceres/block_evaluate_preparer.h
index a786931..354acc0 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 @@
// 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 52a58bb..f90c350 100644
--- a/internal/ceres/block_jacobian_writer.cc
+++ b/internal/ceres/block_jacobian_writer.cc
@@ -136,9 +136,12 @@
// 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 adefdd2..e80efa1 100644
--- a/internal/ceres/evaluator.h
+++ b/internal/ceres/evaluator.h
@@ -95,6 +95,7 @@
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 b7381a4..6fbb759 100644
--- a/internal/ceres/evaluator_test.cc
+++ b/internal/ceres/evaluator_test.cc
@@ -88,6 +88,15 @@
}
};
+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<pair<LinearSolverType, int> > {
Evaluator* CreateEvaluator(Program* program) {
@@ -103,6 +112,93 @@
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> 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));
+
+ 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 @@
}
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> evaluator(CreateEvaluator(problem.mutable_program()));
- scoped_ptr<SparseMatrix> 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 @@
NULL,
z, y, x);
- scoped_ptr<Evaluator> evaluator(CreateEvaluator(problem.mutable_program()));
- scoped_ptr<SparseMatrix> 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> evaluator(CreateEvaluator(problem.mutable_program()));
- scoped_ptr<SparseMatrix> 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 @@
NULL,
y, z);
- scoped_ptr<Evaluator> evaluator(CreateEvaluator(problem.mutable_program()));
- scoped_ptr<SparseMatrix> 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 @@
/* 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 @@
NULL,
y, z);
- scoped_ptr<Evaluator> evaluator(CreateEvaluator(problem.mutable_program()));
- scoped_ptr<SparseMatrix> 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 @@
/* 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 @@
// 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<ParameterBlock*>* 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> evaluator(CreateEvaluator(&reduced_program));
- scoped_ptr<SparseMatrix> 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 @@
/* 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> evaluator(CreateEvaluator(problem.mutable_program()));
scoped_ptr<SparseMatrix> 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 @@
// 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 @@
{
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 @@
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 3d62272..f6704e4 100644
--- a/internal/ceres/program.cc
+++ b/internal/ceres/program.cc
@@ -201,6 +201,15 @@
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 1386d3c..61fa06b 100644
--- a/internal/ceres/program.h
+++ b/internal/ceres/program.h
@@ -108,6 +108,7 @@
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 b309aad..6c48e7d 100644
--- a/internal/ceres/program_evaluator.h
+++ b/internal/ceres/program_evaluator.h
@@ -120,6 +120,7 @@
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 @@
// 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 @@
// 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 @@
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 @@
}
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<double> scratch;
+ scoped_array<double> residual_block_evaluate_scratch;
+ // The gradient in the local parameterization.
+ scoped_array<double> gradient;
+ // Enough space to store the residual for the largest residual block.
+ scoped_array<double> residual_block_residuals;
scoped_array<double*> jacobian_block_ptrs;
};
@@ -260,11 +307,16 @@
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 c475690..f990b80 100644
--- a/internal/ceres/trust_region_minimizer.cc
+++ b/internal/ceres/trust_region_minimizer.cc
@@ -161,7 +161,7 @@
// 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 @@
// 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 @@
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 03297c0..414567a 100644
--- a/internal/ceres/trust_region_minimizer_test.cc
+++ b/internal/ceres/trust_region_minimizer_test.cc
@@ -83,6 +83,7 @@
virtual bool Evaluate(const double* state,
double* cost,
double* residuals,
+ double* /* gradient */,
SparseMatrix* jacobian) {
double x1 = state[0];
double x2 = state[1];
