Use Program::RemoveFixedBlocks. And get rid of the code and tests related to SolverImpl::RemovedFixedBlocksFromProgram. Change-Id: Iccf6501dfe93db737f5a2265bbab0021712be92f
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc index 30d5c16..9b39156 100644 --- a/internal/ceres/solver_impl.cc +++ b/internal/ceres/solver_impl.cc
@@ -741,107 +741,6 @@ return true; } - -// Strips varying parameters and residuals, maintaining order, and updating -// orderings. -bool SolverImpl::RemoveFixedBlocksFromProgram( - Program* program, - ParameterBlockOrdering* linear_solver_ordering, - ParameterBlockOrdering* inner_iteration_ordering, - double* fixed_cost, - string* error) { - scoped_array<double> residual_block_evaluate_scratch; - if (fixed_cost != NULL) { - residual_block_evaluate_scratch.reset( - new double[program->MaxScratchDoublesNeededForEvaluate()]); - *fixed_cost = 0.0; - } - - vector<ParameterBlock*>* parameter_blocks = - program->mutable_parameter_blocks(); - vector<ResidualBlock*>* residual_blocks = - program->mutable_residual_blocks(); - - // 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) { - (*parameter_blocks)[i]->set_index(-1); - } - - // Filter out residual that have all-constant parameters, and mark all the - // parameter blocks that appear in residuals. - int num_active_residual_blocks = 0; - for (int i = 0; i < residual_blocks->size(); ++i) { - ResidualBlock* residual_block = (*residual_blocks)[i]; - int num_parameter_blocks = residual_block->NumParameterBlocks(); - - // Determine if the residual block is fixed, and also mark varying - // parameters that appear in the residual block. - bool all_constant = true; - for (int k = 0; k < num_parameter_blocks; k++) { - ParameterBlock* parameter_block = residual_block->parameter_blocks()[k]; - if (!parameter_block->IsConstant()) { - all_constant = false; - parameter_block->set_index(1); - } - } - - if (!all_constant) { - (*residual_blocks)[num_active_residual_blocks++] = residual_block; - } 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(true, - &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(num_active_residual_blocks); - - // Filter out unused or fixed parameter blocks, and update the - // linear_solver_ordering and the inner_iteration_ordering (if - // present). - int num_active_parameter_blocks = 0; - for (int i = 0; i < parameter_blocks->size(); ++i) { - ParameterBlock* parameter_block = (*parameter_blocks)[i]; - if (parameter_block->index() == -1) { - // Parameter block is constant. - if (linear_solver_ordering != NULL) { - linear_solver_ordering->Remove(parameter_block->mutable_user_state()); - } - - // It is not necessary that the inner iteration ordering contain - // this parameter block. But calling Remove is safe, as it will - // just return false. - if (inner_iteration_ordering != NULL) { - inner_iteration_ordering->Remove(parameter_block->mutable_user_state()); - } - continue; - } - - (*parameter_blocks)[num_active_parameter_blocks++] = parameter_block; - } - parameter_blocks->resize(num_active_parameter_blocks); - - if (!(((program->NumResidualBlocks() == 0) && - (program->NumParameterBlocks() == 0)) || - ((program->NumResidualBlocks() != 0) && - (program->NumParameterBlocks() != 0)))) { - *error = "Congratulations, you found a bug in Ceres. Please report it."; - return false; - } - - return true; -} - Program* SolverImpl::CreateReducedProgram(Solver::Options* options, ProblemImpl* problem_impl, double* fixed_cost, @@ -854,14 +753,18 @@ options->linear_solver_ordering.get(); const int min_group_id = linear_solver_ordering->group_to_elements().begin()->first; + vector<double*> removed_parameter_blocks; + if (!transformed_program->RemoveFixedBlocks(&removed_parameter_blocks, + fixed_cost, + error)) { + return NULL; + } + + linear_solver_ordering->Remove(removed_parameter_blocks); ParameterBlockOrdering* inner_iteration_ordering = options->inner_iteration_ordering.get(); - if (!RemoveFixedBlocksFromProgram(transformed_program.get(), - linear_solver_ordering, - inner_iteration_ordering, - fixed_cost, - error)) { - return NULL; + if (inner_iteration_ordering != NULL) { + inner_iteration_ordering->Remove(removed_parameter_blocks); } VLOG(2) << "Reduced problem: "
diff --git a/internal/ceres/solver_impl_test.cc b/internal/ceres/solver_impl_test.cc index 9e14d7e..c22ac49 100644 --- a/internal/ceres/solver_impl_test.cc +++ b/internal/ceres/solver_impl_test.cc
@@ -64,7 +64,9 @@ virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { - // Do nothing. This is never called. + for (int i = 0; i < kNumResiduals; ++i) { + residuals[i] = 0.0; + } return true; } }; @@ -73,235 +75,6 @@ class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {}; class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {}; -TEST(SolverImpl, RemoveFixedBlocksNothingConstant) { - ProblemImpl problem; - double x; - double y; - double z; - - problem.AddParameterBlock(&x, 1); - problem.AddParameterBlock(&y, 1); - problem.AddParameterBlock(&z, 1); - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); - problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z); - - string message; - { - ParameterBlockOrdering linear_solver_ordering; - linear_solver_ordering.AddElementToGroup(&x, 0); - linear_solver_ordering.AddElementToGroup(&y, 0); - linear_solver_ordering.AddElementToGroup(&z, 0); - - ParameterBlockOrdering inner_iteration_ordering; - inner_iteration_ordering.AddElementToGroup(&x, 0); - inner_iteration_ordering.AddElementToGroup(&y, 0); - inner_iteration_ordering.AddElementToGroup(&z, 0); - - Program program(*problem.mutable_program()); - EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram( - &program, - &linear_solver_ordering, - &inner_iteration_ordering, - NULL, - &message)); - EXPECT_EQ(program.NumParameterBlocks(), 3); - EXPECT_EQ(program.NumResidualBlocks(), 3); - EXPECT_EQ(linear_solver_ordering.NumElements(), 3); - EXPECT_EQ(inner_iteration_ordering.NumElements(), 3); - } -} - -TEST(SolverImpl, RemoveFixedBlocksAllParameterBlocksConstant) { - ProblemImpl problem; - double x; - - problem.AddParameterBlock(&x, 1); - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); - problem.SetParameterBlockConstant(&x); - - ParameterBlockOrdering linear_solver_ordering; - linear_solver_ordering.AddElementToGroup(&x, 0); - - ParameterBlockOrdering inner_iteration_ordering; - inner_iteration_ordering.AddElementToGroup(&x, 0); - - Program program(problem.program()); - string message; - EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram( - &program, - &linear_solver_ordering, - &inner_iteration_ordering, - NULL, - &message)); - EXPECT_EQ(program.NumParameterBlocks(), 0); - EXPECT_EQ(program.NumResidualBlocks(), 0); - EXPECT_EQ(linear_solver_ordering.NumElements(), 0); - EXPECT_EQ(inner_iteration_ordering.NumElements(), 0); -} - -TEST(SolverImpl, RemoveFixedBlocksNoResidualBlocks) { - ProblemImpl problem; - double x; - double y; - double z; - - problem.AddParameterBlock(&x, 1); - problem.AddParameterBlock(&y, 1); - problem.AddParameterBlock(&z, 1); - - ParameterBlockOrdering linear_solver_ordering; - linear_solver_ordering.AddElementToGroup(&x, 0); - linear_solver_ordering.AddElementToGroup(&y, 0); - linear_solver_ordering.AddElementToGroup(&z, 0); - - ParameterBlockOrdering inner_iteration_ordering; - inner_iteration_ordering.AddElementToGroup(&x, 0); - inner_iteration_ordering.AddElementToGroup(&y, 0); - inner_iteration_ordering.AddElementToGroup(&z, 0); - - Program program(problem.program()); - string message; - EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram( - &program, - &linear_solver_ordering, - &inner_iteration_ordering, - NULL, - &message)); - EXPECT_EQ(program.NumParameterBlocks(), 0); - EXPECT_EQ(program.NumResidualBlocks(), 0); - EXPECT_EQ(linear_solver_ordering.NumElements(), 0); - EXPECT_EQ(inner_iteration_ordering.NumElements(), 0); -} - -TEST(SolverImpl, RemoveFixedBlocksOneParameterBlockConstant) { - ProblemImpl problem; - double x; - double y; - double z; - - problem.AddParameterBlock(&x, 1); - problem.AddParameterBlock(&y, 1); - problem.AddParameterBlock(&z, 1); - - ParameterBlockOrdering linear_solver_ordering; - linear_solver_ordering.AddElementToGroup(&x, 0); - linear_solver_ordering.AddElementToGroup(&y, 0); - linear_solver_ordering.AddElementToGroup(&z, 0); - - ParameterBlockOrdering inner_iteration_ordering; - inner_iteration_ordering.AddElementToGroup(&x, 0); - inner_iteration_ordering.AddElementToGroup(&y, 0); - inner_iteration_ordering.AddElementToGroup(&z, 0); - - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); - problem.SetParameterBlockConstant(&x); - - - Program program(problem.program()); - string message; - EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram( - &program, - &linear_solver_ordering, - &inner_iteration_ordering, - NULL, - &message)); - EXPECT_EQ(program.NumParameterBlocks(), 1); - EXPECT_EQ(program.NumResidualBlocks(), 1); - EXPECT_EQ(linear_solver_ordering.NumElements(), 1); - EXPECT_EQ(inner_iteration_ordering.NumElements(), 1); -} - -TEST(SolverImpl, RemoveFixedBlocksNumEliminateBlocks) { - ProblemImpl problem; - double x; - double y; - double z; - - problem.AddParameterBlock(&x, 1); - problem.AddParameterBlock(&y, 1); - problem.AddParameterBlock(&z, 1); - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); - problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); - problem.SetParameterBlockConstant(&x); - - ParameterBlockOrdering linear_solver_ordering; - linear_solver_ordering.AddElementToGroup(&x, 0); - linear_solver_ordering.AddElementToGroup(&y, 0); - linear_solver_ordering.AddElementToGroup(&z, 1); - - ParameterBlockOrdering inner_iteration_ordering; - inner_iteration_ordering.AddElementToGroup(&x, 0); - inner_iteration_ordering.AddElementToGroup(&y, 0); - inner_iteration_ordering.AddElementToGroup(&z, 1); - - Program program(problem.program()); - string message; - EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram( - &program, - &linear_solver_ordering, - &inner_iteration_ordering, - NULL, - &message)); - EXPECT_EQ(program.NumParameterBlocks(), 2); - EXPECT_EQ(program.NumResidualBlocks(), 2); - EXPECT_EQ(linear_solver_ordering.NumElements(), 2); - EXPECT_EQ(linear_solver_ordering.GroupId(&y), 0); - EXPECT_EQ(linear_solver_ordering.GroupId(&z), 1); - EXPECT_EQ(inner_iteration_ordering.NumElements(), 2); - EXPECT_EQ(inner_iteration_ordering.GroupId(&y), 0); - EXPECT_EQ(inner_iteration_ordering.GroupId(&z), 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); - - ParameterBlockOrdering linear_solver_ordering; - linear_solver_ordering.AddElementToGroup(&x, 0); - linear_solver_ordering.AddElementToGroup(&y, 0); - linear_solver_ordering.AddElementToGroup(&z, 1); - - 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(true, - &expected_fixed_cost, - NULL, - NULL, - scratch.get()); - - string message; - EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram( - &program, - &linear_solver_ordering, - NULL, - &fixed_cost, - &message)); - EXPECT_EQ(program.NumParameterBlocks(), 2); - EXPECT_EQ(program.NumResidualBlocks(), 2); - EXPECT_EQ(linear_solver_ordering.NumElements(), 2); - EXPECT_EQ(linear_solver_ordering.GroupId(&y), 0); - EXPECT_EQ(linear_solver_ordering.GroupId(&z), 1); - EXPECT_DOUBLE_EQ(fixed_cost, expected_fixed_cost); -} - TEST(SolverImpl, ReorderResidualBlockNormalFunction) { ProblemImpl problem; double x; @@ -393,8 +166,12 @@ // Create the reduced program. This should remove the fixed block "z", // marking the index to -1 at the same time. x and y also get indices. string message; + double fixed_cost; scoped_ptr<Program> reduced_program( - SolverImpl::CreateReducedProgram(&options, &problem, NULL, &message)); + SolverImpl::CreateReducedProgram(&options, + &problem, + &fixed_cost, + &message)); const vector<ResidualBlock*>& residual_blocks = problem.program().residual_blocks(); @@ -460,8 +237,12 @@ options.linear_solver_ordering.reset(linear_solver_ordering); string message; + double fixed_cost; scoped_ptr<Program> reduced_program( - SolverImpl::CreateReducedProgram(&options, &problem, NULL, &message)); + SolverImpl::CreateReducedProgram(&options, + &problem, + &fixed_cost, + &message)); const vector<ResidualBlock*>& residual_blocks = reduced_program->residual_blocks();