Refactor SolverImpl::CreateReducedProgram.

Break up CreateReducedProgram into smaller functions in
preparation for more sophisticated ordering strategies.

Change-Id: Ic3897522574fde770646d747fe383f5dbd7a6619

internal/ceres/solver_impl.cc

@@ -38,8 +38,8 @@
 #include "ceres/gradient_checking_cost_function.h"
 #include "ceres/iteration_callback.h"
 #include "ceres/levenberg_marquardt_strategy.h"
-#include "ceres/linear_solver.h"
 #include "ceres/line_search_minimizer.h"
+#include "ceres/linear_solver.h"
 #include "ceres/map_util.h"
 #include "ceres/minimizer.h"
 #include "ceres/ordered_groups.h"
@@ -505,19 +505,6 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options,
   summary->trust_region_strategy_type = options.trust_region_strategy_type;
   summary->dogleg_type = options.dogleg_type;
 
-  // Only Schur types require the lexicographic reordering.
-  if (IsSchurType(options.linear_solver_type)) {
-    const int num_eliminate_blocks =
-        options.linear_solver_ordering
-        ->group_to_elements().begin()
-        ->second.size();
-    if (!LexicographicallyOrderResidualBlocks(num_eliminate_blocks,
-                                              reduced_program.get(),
-                                              &summary->error)) {
-      return;
-    }
-  }
-
   scoped_ptr<Evaluator> evaluator(CreateEvaluator(options,
                                                   problem_impl->parameter_map(),
                                                   reduced_program.get(),
@@ -953,11 +940,14 @@ bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program,
     parameter_blocks->resize(j);
   }
 
-  CHECK(((program->NumResidualBlocks() == 0) &&
+  if (!(((program->NumResidualBlocks() == 0) &&
          (program->NumParameterBlocks() == 0)) ||
         ((program->NumResidualBlocks() != 0) &&
-         (program->NumParameterBlocks() != 0)))
-      << "Congratulations, you found a bug in Ceres. Please report it.";
+         (program->NumParameterBlocks() != 0)))) {
+    *error =  "Congratulations, you found a bug in Ceres. Please report it.";
+    return false;
+  }
+
   return true;
 }
 
@@ -965,19 +955,14 @@ Program* SolverImpl::CreateReducedProgram(Solver::Options* options,
                                           ProblemImpl* problem_impl,
                                           double* fixed_cost,
                                           string* error) {
-  EventLogger event_logger("CreateReducedProgram");
-
   CHECK_NOTNULL(options->linear_solver_ordering);
   Program* original_program = problem_impl->mutable_program();
   scoped_ptr<Program> transformed_program(new Program(*original_program));
-  event_logger.AddEvent("TransformedProgram");
 
   ParameterBlockOrdering* linear_solver_ordering =
       options->linear_solver_ordering;
-
   const int min_group_id =
       linear_solver_ordering->group_to_elements().begin()->first;
-  const int original_num_groups = linear_solver_ordering->NumGroups();
 
   if (!RemoveFixedBlocksFromProgram(transformed_program.get(),
                                     linear_solver_ordering,
@@ -986,97 +971,39 @@ Program* SolverImpl::CreateReducedProgram(Solver::Options* options,
     return NULL;
   }
 
-  event_logger.AddEvent("RemoveFixedBlocks");
-
   if (transformed_program->NumParameterBlocks() == 0) {
-    if (transformed_program->NumResidualBlocks() > 0) {
-      *error = "Zero parameter blocks but non-zero residual blocks"
-          " in the reduced program. Congratulations, you found a "
-          "Ceres bug! Please report this error to the developers.";
-      return NULL;
-    }
-
     LOG(WARNING) << "No varying parameter blocks to optimize; "
                  << "bailing early.";
     return transformed_program.release();
   }
 
-  // If the user supplied an linear_solver_ordering with just one
-  // group, it is equivalent to the user supplying NULL as
-  // ordering. Ceres is completely free to choose the parameter block
-  // ordering as it sees fit. For Schur type solvers, this means that
-  // the user wishes for Ceres to identify the e_blocks, which we do
-  // by computing a maximal independent set.
-  if (original_num_groups == 1 && IsSchurType(options->linear_solver_type)) {
-    vector<ParameterBlock*> schur_ordering;
-    const int num_eliminate_blocks = ComputeSchurOrdering(*transformed_program,
-                                                          &schur_ordering);
-    CHECK_EQ(schur_ordering.size(), transformed_program->NumParameterBlocks())
-        << "Congratulations, you found a Ceres bug! Please report this error "
-        << "to the developers.";
-
-    for (int i = 0; i < schur_ordering.size(); ++i) {
-      linear_solver_ordering->AddElementToGroup(
-          schur_ordering[i]->mutable_user_state(),
-          (i < num_eliminate_blocks) ? 0 : 1);
-    }
-  }
-  event_logger.AddEvent("SchurOrdering");
-
-  if (!ApplyUserOrdering(problem_impl->parameter_map(),
-                         linear_solver_ordering,
-                         transformed_program.get(),
-                         error)) {
-    return NULL;
-  }
-  event_logger.AddEvent("ApplyOrdering");
-
-  // If the user requested the use of a Schur type solver, and
-  // supplied a non-NULL linear_solver_ordering object with more than
-  // one elimination group, then it can happen that after all the
-  // parameter blocks which are fixed or unused have been removed from
-  // the program and the ordering, there are no more parameter blocks
-  // in the first elimination group.
-  //
-  // In such a case, the use of a Schur type solver is not possible,
-  // as they assume there is at least one e_block. Thus, we
-  // automatically switch to one of the other solvers, depending on
-  // the user's indicated preferences.
   if (IsSchurType(options->linear_solver_type) &&
-      original_num_groups > 1 &&
       linear_solver_ordering->GroupSize(min_group_id) == 0) {
-    string msg = "No e_blocks remaining. Switching from ";
-    if (options->linear_solver_type == SPARSE_SCHUR) {
-      options->linear_solver_type = SPARSE_NORMAL_CHOLESKY;
-      msg += "SPARSE_SCHUR to SPARSE_NORMAL_CHOLESKY.";
-    } else if (options->linear_solver_type == DENSE_SCHUR) {
-      // TODO(sameeragarwal): This is probably not a great choice.
-      // Ideally, we should have a DENSE_NORMAL_CHOLESKY, that can
-      // take a BlockSparseMatrix as input.
-      options->linear_solver_type = DENSE_QR;
-      msg += "DENSE_SCHUR to DENSE_QR.";
-    } else if (options->linear_solver_type == ITERATIVE_SCHUR) {
-      msg += StringPrintf("ITERATIVE_SCHUR with %s preconditioner "
-                          "to CGNR with JACOBI preconditioner.",
-                          PreconditionerTypeToString(
-                              options->preconditioner_type));
-      options->linear_solver_type = CGNR;
-      if (options->preconditioner_type != IDENTITY) {
-        // CGNR currently only supports the JACOBI preconditioner.
-        options->preconditioner_type = JACOBI;
-      }
-    }
-
-    LOG(WARNING) << msg;
+    // If the user requested the use of a Schur type solver, and
+    // supplied a non-NULL linear_solver_ordering object with more than
+    // one elimination group, then it can happen that after all the
+    // parameter blocks which are fixed or unused have been removed from
+    // the program and the ordering, there are no more parameter blocks
+    // in the first elimination group.
+    //
+    // In such a case, the use of a Schur type solver is not possible,
+    // as they assume there is at least one e_block. Thus, we
+    // automatically switch to the closest solver to the one indicated
+    // by the user.
+    AlternateLinearSolverForSchurTypeLinearSolver(options);
   }
 
-  event_logger.AddEvent("AlternateSolver");
+  if (IsSchurType(options->linear_solver_type)) {
+    if (!ReorderProgramForSchurTypeLinearSolver(problem_impl->parameter_map(),
+                                                linear_solver_ordering,
+                                                transformed_program.get(),
+                                                error)) {
+      return NULL;
+    }
+    return transformed_program.release();
+  }
 
-  // Since the transformed program is the "active" program, and it is
-  // mutated, update the parameter offsets and indices.
   transformed_program->SetParameterOffsetsAndIndex();
-
-  event_logger.AddEvent("SetOffsets");
   return transformed_program.release();
 }
 
@@ -1398,5 +1325,96 @@ CoordinateDescentMinimizer* SolverImpl::CreateInnerIterationMinimizer(
   return inner_iteration_minimizer.release();
 }
 
+void SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(
+    Solver::Options* options) {
+  if (!IsSchurType(options->linear_solver_type)) {
+    return;
+  }
+
+  string msg = "No e_blocks remaining. Switching from ";
+  if (options->linear_solver_type == SPARSE_SCHUR) {
+    options->linear_solver_type = SPARSE_NORMAL_CHOLESKY;
+    msg += "SPARSE_SCHUR to SPARSE_NORMAL_CHOLESKY.";
+  } else if (options->linear_solver_type == DENSE_SCHUR) {
+    // TODO(sameeragarwal): This is probably not a great choice.
+    // Ideally, we should have a DENSE_NORMAL_CHOLESKY, that can
+    // take a BlockSparseMatrix as input.
+    options->linear_solver_type = DENSE_QR;
+    msg += "DENSE_SCHUR to DENSE_QR.";
+  } else if (options->linear_solver_type == ITERATIVE_SCHUR) {
+    options->linear_solver_type = CGNR;
+    if (options->preconditioner_type != IDENTITY) {
+      msg += StringPrintf("ITERATIVE_SCHUR with %s preconditioner "
+                          "to CGNR with JACOBI preconditioner.",
+                          PreconditionerTypeToString(
+                            options->preconditioner_type));
+      // CGNR currently only supports the JACOBI preconditioner.
+      options->preconditioner_type = JACOBI;
+    } else {
+      msg += StringPrintf("ITERATIVE_SCHUR with IDENTITY preconditioner "
+                          "to CGNR with IDENTITY preconditioner.");
+    }
+  }
+  LOG(WARNING) << msg;
+}
+
+bool SolverImpl::ReorderProgramForSchurTypeLinearSolver(
+    const ProblemImpl::ParameterMap& parameter_map,
+    ParameterBlockOrdering* ordering,
+    Program* program,
+    string* error) {
+  // At this point one of two things is true.
+  //
+  //  1. The user did not specify an ordering - ordering has one
+  //  group containined all the parameter blocks.
+
+  //  2. The user specified an ordering, and the first group has
+  //  non-zero elements.
+  //
+  // We handle these two cases in turn.
+  if (ordering->NumGroups() == 1) {
+    // If the user supplied an ordering with just one
+    // group, it is equivalent to the user supplying NULL as an
+    // ordering. Ceres is completely free to choose the parameter
+    // block ordering as it sees fit. For Schur type solvers, this
+    // means that the user wishes for Ceres to identify the e_blocks,
+    // which we do by computing a maximal independent set.
+    vector<ParameterBlock*> schur_ordering;
+    const int num_eliminate_blocks = ComputeSchurOrdering(*program,
+                                                          &schur_ordering);
+
+    CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks())
+        << "Congratulations, you found a Ceres bug! Please report this error "
+        << "to the developers.";
+
+    // Update the ordering object.
+    for (int i = 0; i < schur_ordering.size(); ++i) {
+      double* parameter_block = schur_ordering[i]->mutable_user_state();
+      const int group_id = (i < num_eliminate_blocks) ? 0 : 1;
+      ordering->AddElementToGroup(parameter_block, group_id);
+    }
+
+    // Apply the parameter block re-ordering. Technically we could
+    // call ApplyUserOrdering, but this is cheaper and simpler.
+    swap(*program->mutable_parameter_blocks(), schur_ordering);
+  } else {
+    // The user supplied an ordering.
+    if (!ApplyUserOrdering(parameter_map, ordering, program, error)) {
+      return false;
+    }
+  }
+
+  program->SetParameterOffsetsAndIndex();
+
+  const int num_eliminate_blocks =
+      ordering->group_to_elements().begin()->second.size();
+
+  // Schur type solvers also require that their residual blocks be
+  // lexicographically ordered.
+  return LexicographicallyOrderResidualBlocks(num_eliminate_blocks,
+                                              program,
+                                              error);
+}
+
 }  // namespace internal
 }  // namespace ceres

internal/ceres/solver_impl.h

@@ -151,6 +151,32 @@ class SolverImpl {
       const Program& program,
       const ProblemImpl::ParameterMap& parameter_map,
       Solver::Summary* summary);
+
+  // If the linear solver is of Schur type, then replace it with the
+  // closest equivalent linear solver. This is done when the user
+  // requested a Schur type solver but the problem structure makes it
+  // impossible to use one.
+  //
+  // If the linear solver is not of Schur type, the function is a
+  // no-op.
+  static void AlternateLinearSolverForSchurTypeLinearSolver(
+      Solver::Options* options);
+
+  // Schur type solvers require that all parameter blocks eliminated
+  // by the Schur eliminator occur before others and the residuals be
+  // sorted in lexicographic order of their parameter blocks.
+  //
+  // If ordering has atleast two groups, then apply the ordering,
+  // otherwise compute a new ordering using a Maximal Independent Set
+  // algorithm and apply it.
+  //
+  // Upon return, ordering contains the parameter block ordering that
+  // was used to order the program.
+  static bool ReorderProgramForSchurTypeLinearSolver(
+      const ProblemImpl::ParameterMap& parameter_map,
+      ParameterBlockOrdering* ordering,
+      Program* program,
+      string* error);
 };
 
 }  // namespace internal

internal/ceres/solver_impl_test.cc

@@ -378,11 +378,6 @@ TEST(SolverImpl, ReorderResidualBlockNormalFunctionWithFixedBlocks) {
   expected_residual_blocks.push_back(residual_blocks[3]);
   expected_residual_blocks.push_back(residual_blocks[2]);
 
-  EXPECT_TRUE(SolverImpl::LexicographicallyOrderResidualBlocks(
-                  2,
-                  reduced_program.get(),
-                  &error));
-
   EXPECT_EQ(reduced_program->residual_blocks().size(),
             expected_residual_blocks.size());
   for (int i = 0; i < expected_residual_blocks.size(); ++i) {
@@ -794,5 +789,62 @@ TEST(SolverImpl, ProblemIsConstant) {
   EXPECT_EQ(summary.final_cost, 1.0 / 2.0);
 }
 
+TEST(SolverImpl, AlternateLinearSolverForSchurTypeLinearSolver) {
+  Solver::Options options;
+
+  options.linear_solver_type = DENSE_QR;
+  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
+  EXPECT_EQ(options.linear_solver_type, DENSE_QR);
+
+  options.linear_solver_type = DENSE_NORMAL_CHOLESKY;
+  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
+  EXPECT_EQ(options.linear_solver_type, DENSE_NORMAL_CHOLESKY);
+
+  options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
+  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
+  EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY);
+
+  options.linear_solver_type = CGNR;
+  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
+  EXPECT_EQ(options.linear_solver_type, CGNR);
+
+  options.linear_solver_type = DENSE_SCHUR;
+  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
+  EXPECT_EQ(options.linear_solver_type, DENSE_QR);
+
+  options.linear_solver_type = SPARSE_SCHUR;
+  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
+  EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY);
+
+  options.linear_solver_type = ITERATIVE_SCHUR;
+  options.preconditioner_type = IDENTITY;
+  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
+  EXPECT_EQ(options.linear_solver_type, CGNR);
+  EXPECT_EQ(options.preconditioner_type, IDENTITY);
+
+  options.linear_solver_type = ITERATIVE_SCHUR;
+  options.preconditioner_type = JACOBI;
+  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
+  EXPECT_EQ(options.linear_solver_type, CGNR);
+  EXPECT_EQ(options.preconditioner_type, JACOBI);
+
+  options.linear_solver_type = ITERATIVE_SCHUR;
+  options.preconditioner_type = SCHUR_JACOBI;
+  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
+  EXPECT_EQ(options.linear_solver_type, CGNR);
+  EXPECT_EQ(options.preconditioner_type, JACOBI);
+
+  options.linear_solver_type = ITERATIVE_SCHUR;
+  options.preconditioner_type = CLUSTER_JACOBI;
+  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
+  EXPECT_EQ(options.linear_solver_type, CGNR);
+  EXPECT_EQ(options.preconditioner_type, JACOBI);
+
+  options.linear_solver_type = ITERATIVE_SCHUR;
+  options.preconditioner_type = CLUSTER_TRIDIAGONAL;
+  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
+  EXPECT_EQ(options.linear_solver_type, CGNR);
+  EXPECT_EQ(options.preconditioner_type, JACOBI);
+}
 }  // namespace internal
 }  // namespace ceres

internal/ceres/sparse_normal_cholesky_solver.cc

@@ -205,11 +205,11 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse(
 
   if (factor_ == NULL) {
     if (options_.use_block_amd) {
-      factor_ = ss_.BlockAnalyzeCholesky(lhs.get(),
+      factor_ = ss_.BlockAnalyzeCholesky(&lhs,
                                          A->col_blocks(),
                                          A->row_blocks());
     } else {
-      factor_ = ss_.AnalyzeCholesky(lhs.get());
+      factor_ = ss_.AnalyzeCholesky(&lhs);
     }
   }
   event_logger.AddEvent("Analysis");