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>();