Compute summary->fixed_cost while the residual blocks are removed
Change-Id: Ib598316ad21606f1a06db48c341a5f1d69915b2b
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index 28627eb..493a890 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -236,7 +236,7 @@
// evaluator, and the linear solver.
scoped_ptr<Program> reduced_program(
- CreateReducedProgram(&options, problem_impl, &summary->error));
+ CreateReducedProgram(&options, problem_impl, &summary->fixed_cost, &summary->error));
if (reduced_program == NULL) {
return;
}
@@ -321,11 +321,19 @@
// num_eliminate_blocks.
bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program,
int* num_eliminate_blocks,
+ double* fixed_cost,
string* error) {
int original_num_eliminate_blocks = *num_eliminate_blocks;
vector<ParameterBlock*>* parameter_blocks =
program->mutable_parameter_blocks();
+ scoped_array<double> residual_block_evaluate_scratch;
+ if (fixed_cost != NULL) {
+ residual_block_evalute_scratch.reset(
+ new double[program->MaxScratchDoublesNeededForEvaluate()]);
+ *fixed_cost = 0.0;
+ }
+
// Mark all the parameters as unused. Abuse the index member of the parameter
// blocks for the marking.
for (int i = 0; i < parameter_blocks->size(); ++i) {
@@ -355,6 +363,17 @@
if (!all_constant) {
(*residual_blocks)[j++] = (*residual_blocks)[i];
+ } else if (fixed_cost != NULL) {
+ // The residual is constant and will be removed, so its cost is
+ // added to the variable fixed_cost.
+ double cost = 0.0;
+ if (!residual_block->Evaluate(
+ &cost, NULL, NULL, residual_block_evaluate_scratch.get())) {
+ *error = StringPrintf("Evaluation of the residual %d failed during "
+ "removal of fixed residual blocks.", i);
+ return false;
+ }
+ *fixed_cost += cost;
}
}
residual_blocks->resize(j);
@@ -387,6 +406,7 @@
Program* SolverImpl::CreateReducedProgram(Solver::Options* options,
ProblemImpl* problem_impl,
+ double* fixed_cost,
string* error) {
Program* original_program = problem_impl->mutable_program();
scoped_ptr<Program> transformed_program(new Program(*original_program));
@@ -417,6 +437,7 @@
if (!RemoveFixedBlocksFromProgram(transformed_program.get(),
&num_eliminate_blocks,
+ fixed_cost,
error)) {
return NULL;
}
diff --git a/internal/ceres/solver_impl.h b/internal/ceres/solver_impl.h
index 3ba4d3e..11b44de 100644
--- a/internal/ceres/solver_impl.h
+++ b/internal/ceres/solver_impl.h
@@ -56,8 +56,11 @@
// and residuals eliminated, and in the case of automatic schur
// ordering, has the E blocks first in the resulting program, with
// options.num_eliminate_blocks set appropriately.
+ // If fixed_cost is not NULL, the residual blocks that are removed
+ // are evaluated and the sum of their cost is returned in fixed_cost.
static Program* CreateReducedProgram(Solver::Options* options,
ProblemImpl* problem_impl,
+ double* fixed_cost,
string* error);
// Create the appropriate linear solver, taking into account any
@@ -102,8 +105,11 @@
// depending only on fixed parameters from the problem. Also updates
// num_eliminate_blocks, since removed parameters changes the point
// at which the eliminated blocks is valid.
+ // If fixed_cost is not NULL, the residual blocks that are removed
+ // are evaluated and the sum of their cost is returned in fixed_cost.
static bool RemoveFixedBlocksFromProgram(Program* program,
int* num_eliminate_blocks,
+ double* fixed_cost,
string* error);
};
diff --git a/internal/ceres/solver_impl_test.cc b/internal/ceres/solver_impl_test.cc
index 36dd959..a6d6aac 100644
--- a/internal/ceres/solver_impl_test.cc
+++ b/internal/ceres/solver_impl_test.cc
@@ -41,6 +41,20 @@
namespace ceres {
namespace internal {
+// A cost function that sipmply returns its argument.
+class UnaryIdentityCostFunction : public SizedCostFunction<1, 1> {
+ public:
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ residuals[0] = parameters[0][0];
+ if (jacobians != NULL && jacobians[0] != NULL) {
+ jacobians[0][0] = 1.0;
+ }
+ return true;
+ }
+};
+
// Templated base class for the CostFunction signatures.
template <int kNumResiduals, int N0, int N1, int N2>
class MockCostFunctionBase : public
@@ -77,6 +91,7 @@
Program program(*problem.mutable_program());
EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
&num_eliminate_blocks,
+ NULL,
&error));
EXPECT_EQ(program.NumParameterBlocks(), 3);
EXPECT_EQ(program.NumResidualBlocks(), 3);
@@ -90,6 +105,7 @@
Program program(problem.program());
EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
&num_eliminate_blocks,
+ NULL,
&error));
EXPECT_EQ(program.NumParameterBlocks(), 3);
EXPECT_EQ(program.NumResidualBlocks(), 3);
@@ -110,6 +126,7 @@
string error;
EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
&num_eliminate_blocks,
+ NULL,
&error));
EXPECT_EQ(program.NumParameterBlocks(), 0);
EXPECT_EQ(program.NumResidualBlocks(), 0);
@@ -131,6 +148,7 @@
string error;
EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
&num_eliminate_blocks,
+ NULL,
&error));
EXPECT_EQ(program.NumParameterBlocks(), 0);
EXPECT_EQ(program.NumResidualBlocks(), 0);
@@ -155,6 +173,7 @@
string error;
EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
&num_eliminate_blocks,
+ NULL,
&error));
EXPECT_EQ(program.NumParameterBlocks(), 1);
EXPECT_EQ(program.NumResidualBlocks(), 1);
@@ -180,12 +199,47 @@
string error;
EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
&num_eliminate_blocks,
+ NULL,
&error));
EXPECT_EQ(program.NumParameterBlocks(), 2);
EXPECT_EQ(program.NumResidualBlocks(), 2);
EXPECT_EQ(num_eliminate_blocks, 1);
}
+TEST(SolverImpl, RemoveFixedBlocksFixedCost) {
+ ProblemImpl problem;
+ double x = 1.23;
+ double y = 4.56;
+ double z = 7.89;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+ problem.AddResidualBlock(new UnaryIdentityCostFunction(), NULL, &x);
+ problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
+ problem.SetParameterBlockConstant(&x);
+
+ int num_eliminate_blocks = 2;
+ double fixed_cost = 0.0;
+ Program program(problem.program());
+
+ double expected_fixed_cost;
+ ResidualBlock *expected_removed_block = program.residual_blocks()[0];
+ scoped_array<double> scratch(new double[expected_removed_block->NumScratchDoublesForEvaluate()]);
+ expected_removed_block->Evaluate(&expected_fixed_cost, NULL, NULL, scratch.get());
+
+ string error;
+ EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
+ &num_eliminate_blocks,
+ &fixed_cost,
+ &error));
+ EXPECT_EQ(program.NumParameterBlocks(), 2);
+ EXPECT_EQ(program.NumResidualBlocks(), 2);
+ EXPECT_EQ(num_eliminate_blocks, 1);
+ EXPECT_DOUBLE_EQ(fixed_cost, expected_fixed_cost);
+}
+
TEST(SolverImpl, ReorderResidualBlockNonSchurSolver) {
ProblemImpl problem;
double x;
@@ -322,7 +376,7 @@
// marking the index to -1 at the same time. x and y also get indices.
string error;
scoped_ptr<Program> reduced_program(
- SolverImpl::CreateReducedProgram(&options, &problem, &error));
+ SolverImpl::CreateReducedProgram(&options, &problem, NULL, &error));
const vector<ResidualBlock*>& residual_blocks =
problem.program().residual_blocks();
diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc
index f990b80..357d822 100644
--- a/internal/ceres/trust_region_minimizer.cc
+++ b/internal/ceres/trust_region_minimizer.cc
@@ -167,23 +167,6 @@
return;
}
- // Compute the fixed part of the cost.
- //
- // This is a poor way to do this computation. Even if fixed_cost is
- // zero, because we are subtracting two possibly large numbers, we
- // are depending on exact cancellation to give us a zero here. But
- // initial_cost and cost have been computed by two different
- // evaluators. One which runs on the whole problem (in
- // solver_impl.cc) in single threaded mode and another which runs
- // here on the reduced problem, so fixed_cost can (and does) contain
- // some numerical garbage with a relative magnitude of 1e-14.
- //
- // The right way to do this, would be to compute the fixed cost on
- // just the set of residual blocks which are held constant and were
- // removed from the original problem when the reduced problem was
- // constructed.
- summary->fixed_cost = summary->initial_cost - cost;
-
gradient.setZero();
jacobian->LeftMultiply(residuals.data(), gradient.data());
iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>();
