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