Add Block AMD ordering for SPARSE_SCHUR + EIGEN_SPARSE.

Ordering routines for the Schur complement when using EIGEN_SPARSE.
Also integration into SchurComplementSolver.

Part of this CL is also a refactoring of the block jacobian matrix
construction.

Change-Id: I11d665cc7d4867c64190e6fed1118f4d2e13d59b

internal/ceres/reorder_program.cc

diff --git a/internal/ceres/reorder_program.cc b/internal/ceres/reorder_program.cc
index d05f9f9..5b9212f 100644
--- a/internal/ceres/reorder_program.cc
+++ b/internal/ceres/reorder_program.cc
@@ -78,6 +78,28 @@
   return min_parameter_block_position;
 }
 
+#ifdef CERES_USE_EIGEN_SPARSE
+Eigen::SparseMatrix<int> CreateBlockJacobian(
+    const TripletSparseMatrix& block_jacobian_transpose) {
+  typedef Eigen::SparseMatrix<int> SparseMatrix;
+  typedef Eigen::Triplet<int> Triplet;
+
+  const int* rows = block_jacobian_transpose.rows();
+  const int* cols = block_jacobian_transpose.cols();
+  int num_nonzeros = block_jacobian_transpose.num_nonzeros();
+  std::vector<Triplet> triplets;
+  triplets.reserve(num_nonzeros);
+  for (int i = 0; i < num_nonzeros; ++i) {
+    triplets.push_back(Triplet(cols[i], rows[i], 1));
+  }
+
+  SparseMatrix block_jacobian(block_jacobian_transpose.num_cols(),
+                              block_jacobian_transpose.num_rows());
+  block_jacobian.setFromTriplets(triplets.begin(), triplets.end());
+  return block_jacobian;
+}
+#endif
+
 void OrderingForSparseNormalCholeskyUsingSuiteSparse(
     const TripletSparseMatrix& tsm_block_jacobian_transpose,
     const vector<ParameterBlock*>& parameter_blocks,
@@ -157,30 +179,17 @@
   // row sparse matrix representation of the jacobian and go from
   // there. But that is a project for another day.
 
-  const int* rows = tsm_block_jacobian_transpose.rows();
-  const int* cols = tsm_block_jacobian_transpose.cols();
-
   typedef Eigen::SparseMatrix<int> SparseMatrix;
-  typedef Eigen::Triplet<int> Triplet;
-  std::vector<Triplet> triplets;
-  int num_nonzeros = tsm_block_jacobian_transpose.num_nonzeros();
-  triplets.reserve(num_nonzeros);
-  for (int i = 0; i < num_nonzeros; ++i) {
-    triplets.push_back(Triplet(rows[i], cols[i], 1));
-  }
 
-  SparseMatrix block_jacobian_transpose(
-      tsm_block_jacobian_transpose.num_rows(),
-      tsm_block_jacobian_transpose.num_cols());
-  block_jacobian_transpose.setFromTriplets(triplets.begin(),
-                                           triplets.end());
-  SparseMatrix block_hessian =
-      block_jacobian_transpose * block_jacobian_transpose.transpose();
+  const SparseMatrix block_jacobian =
+      CreateBlockJacobian(tsm_block_jacobian_transpose);
+  const SparseMatrix block_hessian =
+      block_jacobian.transpose() * block_jacobian;
 
   Eigen::AMDOrdering<int> amd_ordering;
   Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> perm;
   amd_ordering(block_hessian, perm);
-  for (int i = 0; i < tsm_block_jacobian_transpose.num_rows(); ++i) {
+  for (int i = 0; i < block_hessian.rows(); ++i) {
     ordering[i] = perm.indices()[i];
   }
 #endif  // CERES_USE_EIGEN_SPARSE
@@ -360,6 +369,64 @@
 #endif
 }
 
+void MaybeReorderSchurComplementColumnsUsingEigen(
+    const int size_of_first_elimination_group,
+    const ProblemImpl::ParameterMap& parameter_map,
+    Program* program) {
+#if !EIGEN_VERSION_AT_LEAST(3, 2, 2) || !defined(CERES_USE_EIGEN_SPARSE)
+  return;
+#else
+
+  scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
+      program->CreateJacobianBlockSparsityTranspose());
+
+  typedef Eigen::SparseMatrix<int> SparseMatrix;
+  const SparseMatrix block_jacobian =
+      CreateBlockJacobian(*tsm_block_jacobian_transpose);
+  const int num_rows = block_jacobian.rows();
+  const int num_cols = block_jacobian.cols();
+
+  // Vertically partition the jacobian in parameter blocks of type E
+  // and F.
+  const SparseMatrix E =
+      block_jacobian.block(0,
+                           0,
+                           num_rows,
+                           size_of_first_elimination_group);
+  const SparseMatrix F =
+      block_jacobian.block(0,
+                           size_of_first_elimination_group,
+                           num_rows,
+                           num_cols - size_of_first_elimination_group);
+
+  // Block sparsity pattern of the schur complement.
+  const SparseMatrix block_schur_complement =
+      F.transpose() * F - F.transpose() * E * E.transpose() * F;
+
+  Eigen::AMDOrdering<int> amd_ordering;
+  Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> perm;
+  amd_ordering(block_schur_complement, perm);
+
+  const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();
+  vector<ParameterBlock*> ordering(num_cols);
+
+  // The ordering of the first size_of_first_elimination_group does
+  // not matter, so we preserve the existing ordering.
+  for (int i = 0; i < size_of_first_elimination_group; ++i) {
+    ordering[i] = parameter_blocks[i];
+  }
+
+  // For the rest of the blocks, use the ordering computed using AMD.
+  for (int i = 0; i < block_schur_complement.cols(); ++i) {
+    ordering[size_of_first_elimination_group + i] =
+        parameter_blocks[size_of_first_elimination_group + perm.indices()[i]];
+  }
+
+  swap(*program->mutable_parameter_blocks(), ordering);
+  program->SetParameterOffsetsAndIndex();
+#endif
+}
+
 bool ReorderProgramForSchurTypeLinearSolver(
     const LinearSolverType linear_solver_type,
     const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
@@ -430,19 +497,24 @@
 
   program->SetParameterOffsetsAndIndex();
 
-  if (linear_solver_type == SPARSE_SCHUR &&
-      sparse_linear_algebra_library_type == SUITE_SPARSE) {
-    MaybeReorderSchurComplementColumnsUsingSuiteSparse(
-        *parameter_block_ordering,
-        program);
+  const int size_of_first_elimination_group =
+      parameter_block_ordering->group_to_elements().begin()->second.size();
+
+  if (linear_solver_type == SPARSE_SCHUR) {
+    if (sparse_linear_algebra_library_type == SUITE_SPARSE) {
+      MaybeReorderSchurComplementColumnsUsingSuiteSparse(
+          *parameter_block_ordering,
+          program);
+    } else if (sparse_linear_algebra_library_type == EIGEN_SPARSE) {
+      MaybeReorderSchurComplementColumnsUsingEigen(
+          size_of_first_elimination_group,
+          parameter_map,
+          program);
+    }
   }
 
-
-
   // Schur type solvers also require that their residual blocks be
   // lexicographically ordered.
-  const int size_of_first_elimination_group =
-      parameter_block_ordering->group_to_elements().begin()->second.size();
   if (!LexicographicallyOrderResidualBlocks(size_of_first_elimination_group,
                                             program,
                                             error)) {

internal/ceres/schur_complement_solver.h

diff --git a/internal/ceres/schur_complement_solver.h b/internal/ceres/schur_complement_solver.h
index 723b149..d1efd28 100644
--- a/internal/ceres/schur_complement_solver.h
+++ b/internal/ceres/schur_complement_solver.h
@@ -49,6 +49,7 @@
 
 #ifdef CERES_USE_EIGEN_SPARSE
 #include "Eigen/SparseCholesky"
+#include "Eigen/OrderingMethods"
 #endif
 
 namespace ceres {
@@ -189,7 +190,19 @@
   cs_dis* cxsparse_factor_;
 
 #ifdef CERES_USE_EIGEN_SPARSE
-  typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > SimplicialLDLT;
+
+  // For Eigen versions less than 3.2.2, we cannot pre-order the
+  // Jacobian, so we must use an AMD ordering.
+#if EIGEN_VERSION_AT_LEAST(3,2,2)
+  typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>,
+                                Eigen::Lower,
+                                Eigen::NaturalOrdering<int> > SimplicialLDLT;
+#else
+  typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>,
+                                Eigen::Lower,
+                                Eigen::AMDOrdering<int> > SimplicialLDLT;
+#endif
+
   scoped_ptr<SimplicialLDLT> simplicial_ldlt_;
 #endif